/*  UNFOLDING_AUTOMATIC_2000.C -- */
/* Slice Sampling program for analyzing 2000 Thermometer Data */

/* 
                      = CLINTON  
                       = GORE     
                       = BUSH     
                       = BUCHANAN 
                       = NADER    
                       = MCCAIN   
                       = BRADLEY  
                       = LIEBERMAN
                       = CHENEY   
                       = HILLARY CLINTON
                       = DEMOCRATIC PARTY
                       = REPUBLICAN PARTY
                       = REFORM PARTY
                       = PARTIES IN GENERAL

gcc -o unfolding_automatic_2000 unfolding_automatic_2000.c -lcblas -lclapack

./unfolding_slice
*/

#include <stdio.h>
#include <stdint.h>

//#include <stdio.h>
//#include <stdlib.h>
//#include <math.h>

#include </Users/poole/lbfgs.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <string.h>
#include <vecLib/clapack.h>
#include <vecLib/vBLAS.h>
#define SLICE_W 1
#define SLICE_P 3
//NDIM
#define NS 2                    /*  Number of Dimensions */
#define N 3003  /* 3003 if NS=2, 309 if NS=3, 410 if NS=4, USED IN L-BFGS ROUTINE -- SET EQUAL to ((nrowX+ncolX)*NS)-3 if NS=2; set equal to ((nrowX+ncolX)*NS) - 6 if NS=3; set equal to ((nrowX+ncolX)*NS) - 10 if NS=4*/
#define NDIM 3005                 /* ((nrowX+ncolX)-1)*NS+1  Number of Coordinates Being Estimated + Variance Term*/
#define nrowX 1489                /*  3 Coordinates = 0, 2*103=206 and 206-3, for 203 Coordinates plus Variance = 204 */
#define ncolX 14
#define SIGMAPRIOR 100.0
static double *Y,*X,*XTEMP,*XCOORDS,*XCOORDS2, *CONSTRAINTS,*ZCOORDS,*XCHAIN,*ZCHAIN;
FILE *jp;
FILE *kp;
FILE *fp;
FILE *lp;
FILE *mp;

double keithrulesSIGMASQ(double *, double *);
double keithrulesX(double theta[]);
double keithrulesZ(double theta[]);
double keithrules22(double theta[], int param);
double keithrules33(double theta[], int param);
double sliceZ(double theta[], double thetaLeft[], double thetaRight[], int param, double w, int p);
double sliceX(double theta[], double thetaLeft[], double thetaRight[], int param, double w, int p);
double runif(void);
void double_center(int kpnp, int kpnq, double *, double *, double *, double *);
void xsvd(int kpnp, int kpnq, double *y, double *u, double *lambda, double *vt); 
void xsvd2(int kpnp, int kpnq, double *y, double *rmatrix); 
void xsvdrotate(int kpnp, int kpnq, double *, double *, double *, double *, double *); 
void xsvdrotate2(int kpnp, int kpnq, double *, double *, double *, double *, double *); 
void nelmin ( double fn ( double x[] ), int n, double start[], double xmin[], 
	      double *ynewlo, double reqmin, double step[], int konvge, int kcount, 
	      int *icount, int *numres, int *ifault );
void Least_Squares_Fit(double *, double *);
void mainlbfgs(int kpnp, int kpnq, double *, double *);
void timestamp ( void );


double runif(void){
	return(rand() / ((double)RAND_MAX + 1));
}
int main(void)
{
	int i,ii,iii,j,jj,jjj,kk,kkk,errno,nslice,nburn,idebug;
	int n, konvge, kcount, icount, numres, ifault;
	int ijkp, jsave, jsave2, i2011;
	int itotalXZ, jtotalXZ;
//
  double *thetanow, *XREAD, *ZCOORDS2, *ZCOORDS3, *XZCHAIN, *ZZCHAIN, *ZCOORDS4, *slicesumX, *slicesumsqX, *sdsliceX, *slicesumZ, *slicesumsqZ, *sdsliceZ;
  double *start, *step, *xmin, *udc, *lambdadc, *vtdc, *xstarts, *xcenter, *sumdim;
  double *thetanow2, *thetaL, *thetaR, *rrr, *aasum, *bbsum, *ccsum, *ddsum, *eesum, *kksum;
  double *u, *lambda, *vt, *rotatematrix, *rmatrix;
  double *ATRUE, *BTRUE;
//
  double ssenow, ssenow2, ssenowmean, ssenowsd, ssenowLAST;
  double sumdist, sum1, sum2, sum11, sum22, sum3, sum4;
  double aaa, bbb, ccc, xzero;
  double smeanLAST, ssdLAST, ssdLAST2;
  double time1, timedif;
  double reqmin,ynewlo,sum,summax,summaxsave,sumLogLX,sumLogLZ;
  double sseb4, sseaf,ssemeanA, ssemeanB, ssemeansqA, ssemeansqB;
//
  X = (double *) malloc (nrowX*ncolX*sizeof(double));
  XTEMP = (double *) malloc (nrowX*ncolX*sizeof(double));
  XREAD = (double *) malloc (nrowX*ncolX*sizeof(double));
  Y = (double *) malloc (nrowX*sizeof(double));
  thetanow = (double *) malloc ((NDIM)*sizeof(double));
  thetanow2 = (double *) malloc ((NDIM)*sizeof(double));
  thetaL = (double *) malloc ((NDIM)*sizeof(double));
  thetaR = (double *) malloc ((NDIM)*sizeof(double));
  rrr = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  aasum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  bbsum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  ccsum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  ddsum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  eesum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  kksum = (double *) malloc ((nrowX*ncolX)*sizeof(double));
  slicesumX = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  slicesumsqX = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  sdsliceX = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  slicesumZ = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  slicesumsqZ = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  sdsliceZ = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  XCOORDS = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  XCOORDS2 = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  XCHAIN = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  ZCOORDS = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  ZCHAIN = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  ZCOORDS2 = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  ZCOORDS3 = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  XZCHAIN = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  ATRUE = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  BTRUE = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  ZZCHAIN = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  ZCOORDS4 = (double *) malloc (((NS*(nrowX+ncolX))+1)*sizeof(double));
  CONSTRAINTS = (double *) malloc (((NS*nrowX)+1)*sizeof(double));
  u      = (double *) malloc (NS*NS*sizeof(double));
  lambda = (double *) malloc (NS*NS*sizeof(double));
  vt     = (double *) malloc (NS*NS*sizeof(double));
  rmatrix = (double *) malloc (nrowX*ncolX*sizeof(double));
  /*
Set up Nedler Mead here to do each respondent given the stimuli
coordinates
*/
  n=NS;
//
  start = ( double * ) malloc ( n * sizeof ( double ) );
  step = ( double * ) malloc ( n * sizeof ( double ) );
  xmin = ( double * ) malloc ( n * sizeof ( double ) );
  udc      = (double *) malloc (nrowX*ncolX*sizeof(double));
  lambdadc = (double *) malloc (nrowX*ncolX*sizeof(double));
  vtdc     = (double *) malloc (nrowX*ncolX*sizeof(double));
  xstarts  = (double *) malloc (nrowX*ncolX*sizeof(double));
  xcenter  = (double *) malloc (nrowX*ncolX*sizeof(double));
  sumdim  = (double *) malloc (nrowX*ncolX*sizeof(double));
//
//
/* clock() is part of time.h -- returns the implementation's
 * best approximationto the processor time elapsed since the
 * program was invoke, divide by CLOCKS_PER_SEC to get the time
 * in seconds           */
  time1 = (double) clock();            /* get initial time */
  time1 = time1 / CLOCKS_PER_SEC;      /*    in seconds    */

  /* open files */
  /* jp holds general diagnostics -- it can be quite large
   * */
  jp = fopen("data_unfolding_2000.txt","w");
  /* mp holds the means of the chains for the individuals and the
   * standard errors
   * */
  mp = fopen("slice_unfolding_chains_X_2000.txt","w");
  /* initialise random number generator */
  srand( time(NULL) );
  for (i=0;i<10;i++){
	  printf("random number #%d: %d\n",i,rand());
	  fprintf(jp,"random number #%d: %d\n",i,rand());
  }
  /* kp holds all the estimates of sigma-hat squared
   * */
  kp = fopen("slice_unfolding_2000.txt","w");
  /* lp holds the chains for the stimuli with the means and standard
   * deviations after burn-in at the bottom of the file
   * */
  lp = fopen("slice_unfolding_chains_2000.txt","w");
  if((fp = fopen("elec2000_therms.txt","r"))==NULL)
  {
	  printf("\nUnable to open file elec2000_therms.txt.dat: %s\n", strerror(errno));
	  exit(EXIT_FAILURE);
  }
  else {

	  fprintf(jp," Y and X = \n");
	  for(i=0;i<nrowX;i++)
	  {
		  fscanf(fp,"%lf",&Y[i]);
		  for(j=0;j<ncolX;j++)
		  {
			  fscanf(fp,"%lf",&XREAD[i*ncolX+j]);
		  }
		  fprintf(jp,"%10d %12.6f", i,Y[i]);
		  for(j=0;j<ncolX;j++)
		  {
			  fprintf(jp,"%12.6f",XREAD[i*ncolX+j]);
		  }
		  fprintf(jp,"\n");
	  }
  }
/*
DO TRANSFORMATION TO DISTANCES HERE -- MISSING DATA > 100
*/
  for(i=0;i<nrowX;i++)
  {
	  for(j=0;j<ncolX;j++)
	  {
		  if(XREAD[i*ncolX+j] <= 97)
		  {
			  X[i*ncolX+j] = (100.0-XREAD[i*ncolX+j])/50.0;
		  }
		  /*
KLUDGE TO GUARD AGAINST ZERO DISTANCES IN THE LOG-NORMAL MODEL

*/
		  if(XREAD[i*ncolX+j] == 98)
		  {
			  X[i*ncolX+j] = 0.06;
		  }
		  if(XREAD[i*ncolX+j] == 99)
		  {
			  X[i*ncolX+j] = 0.06;
		  }
		  if(XREAD[i*ncolX+j] == 100)
		  {
			  X[i*ncolX+j] = 0.06;
		  }
		  if(XREAD[i*ncolX+j] > 100)
		  {
			  X[i*ncolX+j] = -999;
		  }
	  }
  }
/*

  CREATE VARIANCE-COVARIANCE MATRIX FROM THE INPUT MATRIX OF DISTANCES
  SO IT CAN BE DECOMPOSED TO OBTAIN STARTING VALUES FOR THE STIMULI
*/
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<ncolX;jj++)
	  {
		  aasum[j*ncolX+jj]=0.0;
		  bbsum[j*ncolX+jj]=0.0;
		  ccsum[j*ncolX+jj]=0.0;
		  ddsum[j*ncolX+jj]=0.0;
		  eesum[j*ncolX+jj]=0.0;
		  kksum[j*ncolX+jj]=0;
	  }
  }
//
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<ncolX;jj++)
	  {
		  for(i=0;i<nrowX;i++)
		  {
			  if(X[i*ncolX+j] >= 0.0)
			  {
				  if(X[i*ncolX+jj] >= 0.0)
				  {
					  aasum[j*ncolX+jj]=aasum[j*ncolX+jj]+X[i*ncolX+j];
					  bbsum[j*ncolX+jj]=bbsum[j*ncolX+jj]+X[i*ncolX+jj];
					  ccsum[j*ncolX+jj]=ccsum[j*ncolX+jj]+X[i*ncolX+j]*X[i*ncolX+j];
					  ddsum[j*ncolX+jj]=ddsum[j*ncolX+jj]+X[i*ncolX+jj]*X[i*ncolX+jj];
					  eesum[j*ncolX+jj]=eesum[j*ncolX+jj]+X[i*ncolX+j]*X[i*ncolX+jj];
					  kksum[j*ncolX+jj]=kksum[j*ncolX+jj]+1;
				  }
			  }
		  }
	  }
  }
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<ncolX;jj++)
	  {
		  aaa=kksum[j*ncolX+jj]*eesum[j*ncolX+jj]-aasum[j*ncolX+jj]*bbsum[j*ncolX+jj];
		  bbb=kksum[j*ncolX+jj]*ccsum[j*ncolX+jj]-aasum[j*ncolX+jj]*aasum[j*ncolX+jj];
		  ccc=kksum[j*ncolX+jj]*ddsum[j*ncolX+jj]-bbsum[j*ncolX+jj]*bbsum[j*ncolX+jj];
		  rrr[j*ncolX+jj]=1-(aaa/sqrt(bbb*ccc));
	  }
  }
//  
  double_center(ncolX,ncolX,rrr,udc,lambdadc,vtdc);
//
  for(jj=0;jj<NS;jj++)
  {
	  for(j=0;j<ncolX;j++)
	  {
		  xstarts[j*NS+jj]=udc[j+jj*ncolX]*sqrt(lambdadc[jj]);
	  }
  }
  // Normalize to Unit Circle
  summax=-99999999.0;
  for(j=0;j<ncolX;j++)
  {
	  sum=0.0;
	  for(jj=0;jj<NS;jj++)
	  {
		  sum=sum+pow(xstarts[j*NS+jj],2.0);
	  }
	  summax=fmax(sum,summax);
  }
  summax=sqrt(summax);
  printf("Maximum Sum %15.6f\n",summax);
  fprintf(jp,"Maximum Sum %15.6f\n",summax);
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  xstarts[j*NS+jj]=xstarts[j*NS+jj]/summax;
		  ZCOORDS[j*NS+jj]=xstarts[j*NS+jj]/summax;
	  }
  }
  printf("Starting Coordinates For Stimuli From Double-Centering\n");
  fprintf(jp,"Starting Coordinates For Stimuli From Double-Centering\n");
//
  for(j=0;j<ncolX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%15.6f",ZCOORDS[j*NS+jj]);
		  fprintf(jp,"%15.6f",ZCOORDS[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
	  
  }
  /*
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
   RUN SIMPLE METRIC LEAST SQUARES UNFOLDING HERE
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
*/
  for(ijkp=0;ijkp<20;ijkp++)
  {
  /*
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
LOOP OVER THE LEGISLATORS TO GET STARTS
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

*/
  for(kk=0;kk<nrowX;kk++)
  {
// TRANSFER DISTANCES INTO TEMP VECTOR AND CHECK FOR MISSING
// DATA
	  kkk=0;
	  for(jj=0;jj<ncolX;jj++)
	  {
		  XTEMP[jj]=X[kk*ncolX+jj];
		  if(XTEMP[jj] < 0.0)kkk=kkk+1;
	  }
//
// MISSING DATA LOOP
//
	  if(kkk <= 7)
	  {

		  for(j=0;j<NS;j++)
		  {
			  if(ijkp==0)start[j]=(2.0*(((double)rand() + 0.5)/((double)RAND_MAX + 1.0))-1.0);
			  if(ijkp > 0)
			  {
				  start[j]=XCOORDS[kk*NS+j];
			  }
		  }
		  reqmin = 0.0001;

		  for ( i = 0; i < NS; i++ )
		  {
			  step[i] = 1.0;
		  }

		  konvge = 1000;
		  kcount = 100000;

		  ynewlo = keithrulesX ( start );


		  nelmin ( keithrulesX, NS, start, xmin, &ynewlo, reqmin, step, 
			   konvge, kcount, &icount, &numres, &ifault );

	  }
 //
 //  END OF MISSING DATA LOOP
 //
	  for ( i = 0; i < NS; i++ )
	  {
		  if(kkk > 7)
		  {
			  xmin[i]=-99.0;
			  ynewlo=-999.0;
		  }
		  XCOORDS[kk*NS+i]=xmin[i];
	  }
	  if(kkk > 7)
	  {
//		  printf("\n");
	  }
	  if(kkk <= 7)
	  {
/*
*/
	  }
//
  }
/*  
     END OF LEGISLATOR STARTS LOOP


   CALCULTE LEAST SQUARES FIT

*/  
  Least_Squares_Fit(XCOORDS,ZCOORDS);

  /*
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
LOOP OVER THE STIMULI TO GET STARTS
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

*/
  for(kk=0;kk<ncolX;kk++)
  {
// TRANSFER DISTANCES INTO TEMP VECTOR AND CHECK FOR MISSING
// DATA
//	  if(kk!=jsave)
//	  {
		  kkk=0;
		  for(jj=0;jj<nrowX;jj++)
		  {
			  XTEMP[jj]=X[jj*ncolX+kk];
			  if(XTEMP[jj] < 0.0)kkk=kkk+1;
		  }
//
// MISSING DATA LOOP -- AT LEAST 200 RESPONDENTS TO SCALE THE STIMULUS
//
		  if(kkk <= (nrowX-200))
	  {

		  for(j=0;j<NS;j++)
		  {
			  start[j]=ZCOORDS[kk*NS+j];
		  }
		  reqmin = 0.0001;

		  for ( i = 0; i < NS; i++ )
		  {
			  step[i] = 1.0;
		  }

		  konvge = 1000;
		  kcount = 100000;
/*
*/
		  fprintf (jp, "\n Starting point Z:\n" );
		  fprintf (jp, "\n" );
		  for ( i = 0; i < NS; i++ )
		  {
			  fprintf (jp, "  %14f\n", start[i] );
		  }

		  ynewlo = keithrulesZ ( start );

		  fprintf (jp, "\n F(X) = %g\n", ynewlo );
			  nelmin ( keithrulesZ, NS, start, xmin, &ynewlo, reqmin, step, 
				   konvge, kcount, &icount, &numres, &ifault );
/*
*/
		  fprintf (jp, "\n Return code IFAULT = %d\n", ifault );
		  fprintf (jp, "\n" );

	  }
 //
 //  END OF MISSING DATA LOOP
 //
	  printf("%5d",kk);
	  for ( i = 0; i < NS; i++ )
	  {
		  if(kkk > (nrowX-200))
		  {
			  xmin[i]=-99.0;
			  ynewlo=-999.0;
		  }
		  printf ( "  %14f", xmin[i] );
		  ZCOORDS[kk*NS+i]=xmin[i];
	  }
	  printf("\n");
//	  fprintf (kp, "  %14f\n", ynewlo );
	  if(kkk <= (nrowX-200))
	  {

//		  fprintf (jp, "\n" );
		  fprintf (jp, "  F(X*) = %g\n", ynewlo );
//
		  fprintf (jp, "\n" );
		  fprintf (jp, "  Number of iterations = %d\n", icount );
		  fprintf (jp, "  Number of restarts =   %d\n", numres );
	  }
//
//	  }
  }
/*  
     END OF STIMULI STARTS LOOP


   CALCULTE LEAST SQUARES FIT

*/  
  Least_Squares_Fit(XCOORDS,ZCOORDS);

//
  }
/*
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  END OF METRIC UNFOLDING LOOP
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
*/
  /*
FIND POINT CLOSEST TO THE ORIGIN AND SET IT TO ORIGIN

*/
//
  summax=99999999.0;
  jsave=9999999;
  for(j=0;j<ncolX;j++)
  {
	  sum=0.0;
	  for(jj=0;jj<NS;jj++)
	  {
		  sum=sum+fabs(ZCOORDS[j*NS+jj]);
	  }
	  summax=fmin(sum,summax);
//	  if(fabs(summax-sum) < .00001)
	  if(sum <= summax)
	  {
		  jsave=j;
		  summaxsave=summax;
	  }
  }
  summax=sqrt(summax);
  printf("Point Closest to Origin, Minimum Sum %5d %15.6f\n",jsave, summaxsave);
  fprintf(jp,"Point Closest to Origin, Minimum Sum %5d %15.6f\n",jsave, summaxsave);
//
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  ZCOORDS2[j*NS+jj]=(ZCOORDS[j*NS+jj]-ZCOORDS[jsave*NS+jj]);
	  }
  }
  for(j=0;j<ncolX*NS;j++)
  {
	  ZCOORDS[j]=ZCOORDS2[j];
  }
  printf(" FINAL STIMULUS COORDINATES\n");
  fprintf(jp," FINAL STIMULUS COORDINATES\n");
  for(j=0;j<ncolX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%15.6f",ZCOORDS[j*NS+jj]);
		  fprintf(jp,"%15.6f",ZCOORDS[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
  }

  xsvd(ncolX,NS,ZCOORDS,udc,lambdadc,vtdc);
//

/*
  SET CONSTRAINTS HERE -- CONSTRAINTS[NS*nrowX]=sigma**2
*/
  for(i=0;i<=NS*ncolX;i++)
  {
	  CONSTRAINTS[i] = 1;
  }
//  SET CONSTRAINTS FOR POINT AT ORIGIN
  for(j=0;j<NS;j++)
  {
	  CONSTRAINTS[jsave*NS+j]=0;
  }
  kk=0;
  for(j=0;j<NS*ncolX;j++)
  {
	  if(CONSTRAINTS[j]>0.0){
		  ZCOORDS2[kk]=ZCOORDS[j];
		  kk=kk+1;
	  }
  }
  for(jj=0;jj<NS;jj++)
  {
	  ZCOORDS2[(ncolX-1)*NS+jj]=0.0;
  }
//
// DO QR DECOMPOSITION BY ROTATING AROUND POINT SET AT ORIGIN TO IMPOSE
//      THE HARD CONSTRAINTS
//
  xsvd2(ncolX-1,NS,ZCOORDS2,rmatrix);
  // Transfer origin back to its correct position
  for(j=0;j<NS;j++)
  {
	  ZCOORDS2[jsave*NS+j]=rmatrix[(ncolX-1)*NS+j];
  }
  kk=0;
  for(j=0;j<NS*ncolX;j++)
  {
	  if(CONSTRAINTS[j]>0.0){
		  ZCOORDS2[j]=rmatrix[kk];
		  kk=kk+1;
	  }
  }
//  
// Set Constraints Corresponding to QR Decomposition
//
  for(j=0;j<NS-1;j++)
  {
	  for(jj=0;jj<=j;jj++)
	  {
		  CONSTRAINTS[(ncolX-NS+1+j)*NS+jj]=0;
	  }
  }
  printf("\nR Matrix\n");
  fprintf(jp,"\nR Matrix\n");
  for(j=0;j<ncolX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",CONSTRAINTS[j*NS+jj]);
		  fprintf(jp,"%10.5f",CONSTRAINTS[j*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",ZCOORDS2[j*NS+jj]);
		  fprintf(jp,"%10.5f",ZCOORDS2[j*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",rmatrix[j*NS+jj]);
		  fprintf(jp,"%10.5f",rmatrix[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
  }
//
//  CALL Limited-Memory Broyden-Fletcher-Goldfarb-Shanno
//     ZCOORDS3 are the starting coordinates
//     Solution Passed back in ZCOORDS4
//
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  ZCOORDS3[j*NS+jj]=ZCOORDS2[j*NS+jj];
	  }
  }
  for(j=0;j<nrowX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  ZCOORDS3[j*NS+jj+ncolX*NS]=XCOORDS[j*NS+jj];
	  }
  }
//  
  mainlbfgs(nrowX,ncolX,ZCOORDS4,ZCOORDS3);
//
  for(j=0;j<ncolX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  ZCOORDS[j*NS+jj]=ZCOORDS4[j*NS+jj];
	  }
  }
  for(j=0;j<nrowX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  XCOORDS2[j*NS+jj]=ZCOORDS4[j*NS+jj+ncolX*NS];
	  }
  }
//
  printf("\nStimui Coordinates From L-BFGS\n");
  fprintf(jp,"\nCoordinates From L-BFGS\n");
  for(j=0;j<ncolX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  printf("%15d",j);
	  fprintf(jp,"%15d",j);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",CONSTRAINTS[j*NS+jj]);
		  fprintf(jp,"%10.5f",CONSTRAINTS[j*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",ZCOORDS[j*NS+jj]);
		  fprintf(jp,"%10.5f",ZCOORDS[j*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",ZCOORDS2[j*NS+jj]);
		  fprintf(jp,"%10.5f",ZCOORDS2[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
  }
// 
  printf("\nIndividual Coordinates From L-BFGS\n");
  fprintf(jp,"\nIndividual Coordinates From L-BFGS\n");
  for(j=0;j<nrowX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  printf("%15.1f",Y[j]);
	  fprintf(jp,"%15.1f",Y[j]);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",XCOORDS2[j*NS+jj]);
		  fprintf(jp,"%10.5f",XCOORDS2[j*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%10.5f",XCOORDS[j*NS+jj]);
		  fprintf(jp,"%10.5f",XCOORDS[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
  }
//  
// STORE STARTING COORDINATES -- ZCOORDS and XCOORDS2 are the Starts
//  
  for(j=0;j<ncolX*NS;j++)
  {
	  ZCOORDS2[j]=ZCOORDS[j];
  }
// 
  for(j=0;j<nrowX*NS;j++)
  {
	  XCOORDS[j]=XCOORDS2[j];
  }
/*
		  RESET CONSTRAINTS HERE -- CONSTRAINTS[NS*nrowX]=sigma**2
*/
  for(i=0;i<=NS*ncolX;i++)
  {
	  CONSTRAINTS[i] = 1;
  }
//  SET CONSTRAINTS FOR POINT AT ORIGIN
  for(j=0;j<NS;j++)
  {
	  CONSTRAINTS[jsave*NS+j]=0;
  }
  printf(" CONSTRAINTS ON THE COORDINATES \n");
  fprintf(jp," CONSTRAINTS ON THE COORDINATES \n");
  for(j=0;j<ncolX;j++)
  {
	  printf("%5d ",j);
	  fprintf(jp,"%5d ",j);
	  for(jj=0;jj<NS;jj++)
	  {
		  printf("%15.6f",CONSTRAINTS[j*NS+jj]);
		  fprintf(jp,"%15.6f",CONSTRAINTS[j*NS+jj]);
	  }
	  printf("\n");
	  fprintf(jp,"\n");
  }

  
/*

  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
      BEGIN SLICE SAMPLER
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&
  &&&&&&&&&&&&&&&&&&&&&&&&&&&&

  INITIALIZE XCHAIN AND ZCHAIN AND SAVE XCOORDS AND ZCOORDS
*/  
  for(i=0;i<NS*nrowX;i++)
  {
	  XCHAIN[i]=XCOORDS[i];
	  ZCHAIN[i]=ZCOORDS[i];
	  slicesumX[i]=0.0;
	  slicesumsqX[i]=0.0;
	  sdsliceX[i]=0.0;
	  slicesumZ[i]=0.0;
	  slicesumsqZ[i]=0.0;
	  sdsliceZ[i]=0.0;
  }
  /*

   GET INITIAL ESTIMATE OF SIGMA**2

*/
  sum=keithrulesSIGMASQ(XCHAIN,ZCHAIN);
  printf(" sigma-hat squared  %15.6f\n",sum);
  fprintf(kp," sigma-hat squared  %15.6f\n",sum);

//  &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
  /*

  BEGINNING OF SLICE SAMPLER MASTER LOOP

    &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
*/
  nslice=100000;
  nburn=10000;
  for(i2011=0;i2011<nslice+nburn;i2011++)
  {
	  if(i2011 % 100 == 0) printf("Iteration %i complete...\n ",i2011+1);
  xzero=0.0;
  smeanLAST=0.0;
  ssdLAST=0.0;
  sumLogLX=0.0;
  sumLogLZ=0.0;
//

  /*  RUN SLICE ON SET UP RANDOM STARTS */
/*
          RESPONDENTS FIRST
*/
  kk=0;
  for(i=0;i<nrowX;i++)
  {
// TRANSFER DISTANCES INTO TEMP VECTOR AND CHECK FOR MISSING
// DATA
	  kkk=0;
	  for(jj=0;jj<ncolX;jj++)
	  {
		  XTEMP[jj]=X[i*ncolX+jj];
		  if(XTEMP[jj] < 0.0)kkk=kkk+1;
	  }
//  INCREMENT kk HERE BECAUSE OF MISSING DATA LOOP
//
	  for(jj=0;jj<NS;jj++)
	  {
		  kk=kk+1;
		  thetanow2[jj]=0.0;
		  thetanow[jj]=0.0;
	  }
	  if(kkk <= 7)
	  {
		  for(jj=0;jj<NS;jj++)
		  {
//	       thetanow[jj]=XCOORDS[i*NS+jj]+(0.25)*(2.0*(((double)rand() + 0.5)/((double)RAND_MAX + 1.0))-1.0);
			  thetanow[jj]=XCHAIN[i*NS+jj];
			  thetanow2[jj]=thetanow[jj];
			  thetaL[jj]=-9999.0;
			  thetaR[jj]= 9999.0;
		  }
		  for(jj=0;jj<NS;jj++)
		  {
			  thetanow2[jj] = sliceX(thetanow2,thetaL,thetaR,jj,SLICE_W,SLICE_P);
			  thetanow[jj]=thetanow2[jj];
			  XCHAIN[i*NS+jj]=thetanow2[jj];
		  }
//	     fprintf(jp,"%5d %5d",i,kk);
	     for(jj=0;jj<NS;jj++)
	     {
//		  fprintf(jp,"%15.6f",XCOORDS[i*NS+jj]);
	     }
	     for(jj=0;jj<NS;jj++)
	     {
//		  fprintf(jp,"%15.6f",thetanow2[jj]);
	     }
	     sum=keithrules22(thetanow2,i);
//	     fprintf(jp,"%20.8f\n",sum);
	     sumLogLX=sumLogLX+sum;
	  }

  }
  /*
END OF SLICE SAMPLER LOOP ON INDIVIDUALS
*/

  /*
  NOW RUN SLICE ON THE STIMULI
*/
  kk=0;
  for(i=0;i<ncolX;i++)
  {
// TRANSFER DISTANCES INTO TEMP VECTOR AND CHECK FOR MISSING
// DATA
	  if(i!=jsave)
	  {
		  kkk=0;
		  for(jj=0;jj<nrowX;jj++)
		  {
			  XTEMP[jj]=X[jj*ncolX+i];
			  if(XTEMP[jj] < 0.0)kkk=kkk+1;
		  }
	  }
//
// MISSING DATA LOOP -- AT LEAST 200 RESPONDENTS TO SCALE THE STIMULUS
//
//  INCREMENT kk HERE BECAUSE OF MISSING DATA LOOP
//
	  for(jj=0;jj<NS;jj++)
	  {
		  kk=kk+1;
		  thetanow2[jj]=0.0;
		  thetanow[jj]=0.0;
	  }
	  if(kkk <= (nrowX-200))
	  {
		  if(i!=jsave)
		  {
			  for(jj=0;jj<NS;jj++)
			  {
//	       thetanow[jj]=ZCOORDS[i*NS+jj]+(0.25)*(2.0*(((double)rand() + 0.5)/((double)RAND_MAX + 1.0))-1.0);
				  thetanow[jj]=ZCHAIN[i*NS+jj];
				  thetanow2[jj]=thetanow[jj];
				  thetaL[jj]=-9999.0;
				  thetaR[jj]= 9999.0;
			  }
			  for(jj=0;jj<NS;jj++)
			  {
				  thetanow2[jj] = sliceZ(thetanow2,thetaL,thetaR,jj,SLICE_W,SLICE_P);
				  thetanow[jj]=thetanow2[jj];
				  ZCHAIN[i*NS+jj]=thetanow2[jj];
			  }
		  }
	  }
	  fprintf(jp,"%5d %5d",i,kk);
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(jp,"%15.6f",ZCOORDS[i*NS+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(jp,"%15.6f",thetanow2[jj]);
	  }
	  sum=0.0;
	  if(i!=jsave)sum=keithrules33(thetanow2,i);
	  fprintf(jp,"%20.8f\n",sum);
	  sumLogLZ=sumLogLZ+sum;

  }
//  printf(" Sum of Log-likelihood X Phase and Z phase %7d %20.8f %20.8ff\n",i2011, sumLogLX, sumLogLZ);
//
// CALL ORTHOGONAL PROCRUSTES ROTATION HERE -- ZCOORDS/XCOORDS (XTRUE) ARE THE TARGETS,
// ZCHAIN/XCHAIN (XCOORDS) IS THE MATRIX TO BE ROTATED, RMATRIX IS THE ROTATION MATRIX
//
//
//
  kk=0;
  for(j=0;j<ncolX*NS;j++)
  {
	  if(CONSTRAINTS[j]>0)
	  {
		  ZCOORDS4[kk]=ZCOORDS[j];
		  ZCOORDS3[kk]=ZCOORDS[j];
		  ZZCHAIN[kk]=ZCHAIN[j];
		  XZCHAIN[kk]=ZCHAIN[j];
		  kk=kk+1;
	  }
  }
  for(j=0;j<nrowX*NS;j++)
  {
	  if(XCOORDS[j]>-99.0)
	  {
		  ZCOORDS3[kk]=XCOORDS[j];
		  XZCHAIN[kk]=XCHAIN[j];
		  kk=kk+1;
	  }
  }
  itotalXZ=kk;
  jtotalXZ=itotalXZ/NS;
  kk=0;
  for(j=0;j<ncolX;j++)
  {
	  idebug=0;
	  for(jj=0;jj<NS;jj++)
	  {
		  if(CONSTRAINTS[j*NS+jj]>0)
		  {
			  idebug=1;
			  ATRUE[jj*jtotalXZ+kk]=ZCOORDS[jj+j*NS];
			  BTRUE[jj*jtotalXZ+kk]=ZCHAIN[jj+j*NS];
		  }
	  }
	  if(idebug==1)kk=kk+1;
  }
  for(j=0;j<nrowX;j++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  idebug=0;
		  if(XCOORDS[j*NS+jj]>-99.0)
		  {
			  idebug=1;
			  ATRUE[jj*jtotalXZ+kk]=XCOORDS[jj+j*NS];
			  BTRUE[jj*jtotalXZ+kk]=XCHAIN[jj+j*NS];
		  }
	  }
	  if(idebug==1)kk=kk+1;
  }
//  
//  Calculate SSE Before Rotation
//
  sum1=0.0;
  sum3=0.0;
  for (j=0;j<itotalXZ;j++)
  {
	  sum1=sum1+pow((ZCOORDS3[j]-XZCHAIN[j]),2.0);
	  sum3=sum3+pow((ATRUE[j]-BTRUE[j]),2.0);
  }
  sum11=0.0;
  for (j=0;j<((ncolX-1)*NS);j++)
  {
	  sum11=sum11+pow((ZCOORDS4[j]-ZZCHAIN[j]),2.0);
  }
  xsvdrotate(ncolX-1,NS,ZCOORDS4,ZZCHAIN,rmatrix,ATRUE,BTRUE);
  sum22=0.0;
  for (j=0;j<((ncolX-1)*NS);j++)
  {
	  sum22=sum22+pow((ZCOORDS4[j]-ZZCHAIN[j]),2.0);
  }
  xsvdrotate2(jtotalXZ,NS,ZCOORDS3,XZCHAIN,rmatrix,ATRUE,BTRUE);
//
// PUT ROTATED CHAIN BACK IN ZCHAIN AND XCHAIN
//
  kk=0;
  for(j=0;j<ncolX*NS;j++)
  {
	  if(CONSTRAINTS[j]<1.0){
		  ZCHAIN[j]=0.0;
	  }
	  if(CONSTRAINTS[j]>0.0){
		  ZCHAIN[j]=XZCHAIN[kk];
		  kk=kk+1;
	  }
  }
  for(j=0;j<nrowX*NS;j++)
  {
	  if(XCOORDS[j]<-98.0)
	  {
		  XCHAIN[j]=-99.0;
	  }
	  if(XCOORDS[j]>-99.0)
	  {
		  XCHAIN[j]=XZCHAIN[kk];
		  kk=kk+1;
	  }
  }
//
//  Calculate SSE After Rotation
//
  sum2=0.0;
  for (j=0;j<itotalXZ;j++)
  {
	  sum2=sum2+pow((ZCOORDS3[j]-XZCHAIN[j]),2.0);
  }
 /*
  Calculate Correlations Dimension by Dimension
*/
  for(jj=0;jj<NS;jj++){
	  aasum[jj]=0.0;
	  bbsum[jj]=0.0;
	  ccsum[jj]=0.0;
	  ddsum[jj]=0.0;
	  eesum[jj]=0.0;
	  kksum[jj]=0.0;
	  for(ii=0;ii<nrowX;ii++){
		  if(XCOORDS[NS*ii+jj] > -99)
		  {
			  aasum[jj]=aasum[jj]+XCHAIN[NS*ii+jj];
			  bbsum[jj]=bbsum[jj]+XCOORDS[NS*ii+jj];
			  ccsum[jj]=ccsum[jj]+XCHAIN[NS*ii+jj]*XCHAIN[NS*ii+jj];
			  ddsum[jj]=ddsum[jj]+XCOORDS[NS*ii+jj]*XCOORDS[NS*ii+jj];
			  eesum[jj]=eesum[jj]+XCOORDS[NS*ii+jj]*XCHAIN[NS*ii+jj];
			  kksum[jj]=kksum[jj]+1;
			  if(i2011 > nburn)
			  {
				  slicesumX[NS*ii+jj]=slicesumX[NS*ii+jj]+XCHAIN[NS*ii+jj];
				  slicesumsqX[NS*ii+jj]=slicesumsqX[NS*ii+jj]+pow(XCHAIN[NS*ii+jj],2.0);
			  }
		  }
	  }
	  aaa=kksum[jj]*eesum[jj]-aasum[jj]*bbsum[jj];
	  bbb=kksum[jj]*ccsum[jj]-aasum[jj]*aasum[jj];
	  ccc=kksum[jj]*ddsum[jj]-bbsum[jj]*bbsum[jj];
	  rrr[jj]=aaa/sqrt(bbb*ccc);
  }
//
  if(i2011 % 100 == 0)
  {

	  printf(" Sum of Log-likelihood X Phase %7d %20.8f %10.1f %10.1f %12.6f %12.6f %15.6f %15.6f %15.6f %15.6f\n",i2011, sumLogLX, kksum[0], kksum[1], rrr[0], rrr[1], sum1, sum3, sum11, sum22);
  }
  fprintf(jp," Sum of Log-likelihood X Phase %7d %20.8f %10.1f %10.1f %12.6f %12.6f %15.6f %15.6f %15.6f %15.6f\n",i2011, sumLogLX, kksum[0], kksum[1], rrr[0], rrr[1], sum1, sum3, sum11, sum22);
/*
  Do Sign Flips Using ZCOORDS as Target
*/
 /*
  Calculate Correlations Dimension by Dimension
*/
  for(jj=0;jj<NS;jj++){
	  aasum[jj]=0.0;
	  bbsum[jj]=0.0;
	  ccsum[jj]=0.0;
	  ddsum[jj]=0.0;
	  eesum[jj]=0.0;
	  kksum[jj]=0.0;
	  for(ii=0;ii<ncolX;ii++){
		  if(CONSTRAINTS[NS*ii+jj]>0)
		  {
			  aasum[jj]=aasum[jj]+ZCHAIN[NS*ii+jj];
			  bbsum[jj]=bbsum[jj]+ZCOORDS[NS*ii+jj];
			  ccsum[jj]=ccsum[jj]+ZCHAIN[NS*ii+jj]*ZCHAIN[NS*ii+jj];
			  ddsum[jj]=ddsum[jj]+ZCOORDS[NS*ii+jj]*ZCOORDS[NS*ii+jj];
			  eesum[jj]=eesum[jj]+ZCOORDS[NS*ii+jj]*ZCHAIN[NS*ii+jj];
			  kksum[jj]=kksum[jj]+1;
			  if(i2011 > nburn)
			  {
				  slicesumZ[NS*ii+jj]=slicesumZ[NS*ii+jj]+ZCHAIN[NS*ii+jj];
				  slicesumsqZ[NS*ii+jj]=slicesumsqZ[NS*ii+jj]+pow(ZCHAIN[NS*ii+jj],2.0);
			  }
		  }
	  }
	  aaa=kksum[jj]*eesum[jj]-aasum[jj]*bbsum[jj];
	  bbb=kksum[jj]*ccsum[jj]-aasum[jj]*aasum[jj];
	  ccc=kksum[jj]*ddsum[jj]-bbsum[jj]*bbsum[jj];
	  rrr[jj]=aaa/sqrt(bbb*ccc);
  }
  /*

   COMPUTE CORRELATIONS BETWEEN CHAIN POINT COORDINATES
*/
  /*
     WRITE OUT CHAIN

*/
  if(i2011 % 100 == 0)
  {

	  printf(" Sum of Log-likelihood Z Phase %7d %20.8f %10.1f %10.1f %12.6f %12.6f %15.6f %15.6f %15.6f %15.6f\n",i2011, sumLogLZ, kksum[0], kksum[1], rrr[0], rrr[1], sum1, sum2, sum11, sum22);
  }
  fprintf(jp," Sum of Log-likelihood Z Phase %7d %20.8f %10.1f %10.1f %12.6f %12.6f %15.6f %15.6f %15.6f %15.6f\n",i2011, sumLogLZ, kksum[0], kksum[1], rrr[0], rrr[1], sum1, sum2, sum11, sum22);
//

  fprintf(lp,"%7d %20.8f",i2011, sumLogLZ);
  for(jjj=0;jjj<ncolX;jjj++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(lp,"%15.6f",ZCHAIN[jjj*NS+jj]);
	  }
  }
  fprintf(lp,"\n");
  sum=keithrulesSIGMASQ(XCHAIN,ZCHAIN);
//
//  SIMPLE METHOD OF STOPPING EXECUTION
//
//  i2011=100;
//  if(i2011==100)idebug=99998;
//  if(idebug==99998)exit(EXIT_FAILURE);
//
  if(i2011 % 100 == 0)printf(" sigma-hat squared %10d  %15.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f\n",i2011, sum, rrr[0], rrr[1], rmatrix[0], rmatrix[1], rmatrix[2], rmatrix[3]);
  fprintf(kp," sigma-hat squared %10d  %15.6f\n",i2011, sum);
//  if(i2011==101)idebug=99998;
//  if(idebug==99998)exit(EXIT_FAILURE);
  
  }

  /*
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
   END OF SLICE SAMPLER MASTER LOOP
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
*/
/*

  COMPUTE THE MEAN AND STANDARD DEVIATION OF THE CHAINS

*/

  for(jj=0;jj<NS;jj++)
  {
	  for(ii=0;ii<nrowX;ii++)
	  {
		  if(XCOORDS[NS*ii+jj] > -99)
		  {
			  slicesumX[NS*ii+jj]=(slicesumX[NS*ii+jj]/nslice);
			  sdsliceX[NS*ii+jj]=sqrt((slicesumsqX[NS*ii+jj]/nslice) - slicesumX[NS*ii+jj]*slicesumX[NS*ii+jj]);
		  }		  
	  }
	  for(ii=0;ii<ncolX;ii++)
	  {
		  if(CONSTRAINTS[NS*ii+jj]>0)
		  {
			  slicesumZ[NS*ii+jj]=(slicesumZ[NS*ii+jj]/nslice);
			  sdsliceZ[NS*ii+jj]=sqrt((slicesumsqZ[NS*ii+jj]/nslice) - slicesumZ[NS*ii+jj]*slicesumZ[NS*ii+jj]);
		  }
	  }
  }
//  WRITE OUT MEANS OF STANDARD DEVIATIONS OF X CHAINS
//
  for(ii=0;ii<nrowX;ii++)
  {
	  fprintf(mp,"%12.2f",Y[ii]);
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(mp,"%15.6f",XCOORDS[NS*ii+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(mp,"%15.6f",slicesumX[NS*ii+jj]);
	  }
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(mp,"%15.6f",sdsliceX[NS*ii+jj]);
	  }
	  fprintf(mp,"\n");
  }
//
// WRITE OUT MEANS AND STANDARD DEVIATIONS OF Z CHAINS

  fprintf(lp,"%7d %20.8f",i2011, sumLogLZ);
  for(jjj=0;jjj<ncolX;jjj++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(lp,"%15.6f",slicesumZ[jjj*NS+jj]);
	  }
  }
  fprintf(lp,"\n");


  fprintf(lp,"%7d %20.8f",i2011, sumLogLZ);
  for(jjj=0;jjj<ncolX;jjj++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(lp,"%15.6f",sdsliceZ[jjj*NS+jj]);
	  }
  }
  fprintf(lp,"\n");

  
// WRITE OUT "TRUE" COORDINATES AT BOTTOM OF FILE

  fprintf(lp,"%7d %20.8f",i2011, sumLogLZ);
  for(jjj=0;jjj<ncolX;jjj++)
  {
	  for(jj=0;jj<NS;jj++)
	  {
		  fprintf(lp,"%15.6f",ZCOORDS[jjj*NS+jj]);
	  }
  }
  fprintf(lp,"\n");
/*
  CALCULATE THE PEARSON CORRELATION BETWEEN THE ELEMENTS OF THE CHAINS
*/
  // WRITE OUT CORRELATIONS BETWEEN COORDINATES WITHIN POINTS
//
  /*
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
   END OF SLICE SAMPLER MASTER LOOP
     &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
*/

//   
  free(X);
  free(XTEMP);
  free(XREAD);
  free(Y);
  free(thetanow);
  free(thetanow2);
  free(thetaL);
  free(thetaR);
  free(rrr);
  free(aasum);
  free(bbsum);
  free(ccsum);
  free(ddsum);
  free(eesum);
  free(kksum);
  free(slicesumX);
  free(slicesumsqX);
  free(sdsliceX);
  free(slicesumZ);
  free(slicesumsqZ);
  free(sdsliceZ);
  free(XCOORDS);
  free(XCOORDS2);
  free(XCHAIN);
  free(ZCOORDS);
  free(ZCHAIN);
  free(ZCOORDS2);
  free(ZCOORDS3);
  free(XZCHAIN);
  free(ATRUE);
  free(BTRUE);
  free(ZZCHAIN);
  free(ZCOORDS4);
  free(CONSTRAINTS);
  free(start);
  free(step);
  free(xmin);
  free(udc);
  free(lambdadc);
  free(vtdc);
  free(xstarts);
  free(xcenter);
  free(sumdim);
  free(u);
  free(lambda);
  free(vt);
  free(rmatrix);
  timedif = ( ((double) clock()) / CLOCKS_PER_SEC) - time1;
  printf("\nThe total elapsed time of the program is %12.3f seconds\n", timedif);
  fprintf(jp,"\nThe total elapsed time of the program is %12.3f seconds\n", timedif);
  timestamp ( );
  fclose(fp);
  fclose(jp);
  fclose(kp);
  fclose(lp);
  fclose(mp);
  return 0;
}
/*

   SLICE SAMPLER FOR STIMULI COORDINATES
*/
double sliceZ(double theta[], double thetaLeft[], double thetaRight[], int param, double w, int p){

	double x,y,L,R,leftloglike,rightloglike;
	int K;
	short int flag;
	double *temp;
	double rexp = -1*log(runif());
	temp = (double *) malloc((NDIM)*sizeof(double));

	for(K=0;K<NS;K++){
		temp[K]=theta[K];
	}
// w fixed at 1.0, K is fixed at 3
	y = keithrules33(theta,param) - rexp;
	L = theta[param] - w*runif();
	if(L<thetaLeft[param])L=thetaLeft[param]+0.000001;
	R = L + w;
	if(R>thetaRight[param]){
		R=thetaRight[param]-0.000001;
		if(L>R)L=thetaLeft[param]+0.000001;
	}
	K = p;

	temp[param] = L;
	leftloglike = keithrules33(temp,param);
	temp[param] = R;
	rightloglike = keithrules33(temp,param);

	flag=0;
	if(y < leftloglike) flag=1;
	else if(y < rightloglike) flag=-1;

	while(K>0 && flag!=0) {
		if(runif()<0.5) {
			L = 2*L - R;
			if(L<thetaLeft[param])L=thetaLeft[param]+0.000001;
			temp[param] = L;
			if(y >=keithrules33(temp,param) && flag==1) flag=0; 
		}
		else {
			R = 2*R -L;
			if(R>thetaRight[param])R=thetaRight[param]-0.000001;
			temp[param] = R;
			if(y >=keithrules33(temp,param) && flag==-1) flag=0; 
		}
		K--;   
	}

	while(1){
		x = L + runif()*(R-L);
		if(x>thetaRight[param])x=thetaRight[param]-0.000001;
		if(x<thetaLeft[param])x=thetaLeft[param]+0.000001;
		temp[param] = x;
		if(keithrules33(temp,param)>y) break;
		if(theta[param] > x) L = x;
		else R = x;
	} 

	free(temp);
	return(x);

}
//End of Slice

/*

   SLICE SAMPLER FOR INDIVIDUAL COORDINATES
*/

double sliceX(double theta[], double thetaLeft[], double thetaRight[], int param, double w, int p){

	double x,y,L,R,leftloglike,rightloglike;
	int K;
	short int flag;
	double *temp;
	double rexp = -1*log(runif());
	temp = (double *) malloc((NDIM)*sizeof(double));

	for(K=0;K<NS;K++){
		temp[K]=theta[K];
	}
// w fixed at 1.0, K is fixed at 3
	y = keithrules22(theta,param) - rexp;
	L = theta[param] - w*runif();
	if(L<thetaLeft[param])L=thetaLeft[param]+0.000001;
	R = L + w;
	if(R>thetaRight[param]){
		R=thetaRight[param]-0.000001;
		if(L>R)L=thetaLeft[param]+0.000001;
	}
	K = p;

	temp[param] = L;
	leftloglike = keithrules22(temp,param);
	temp[param] = R;
	rightloglike = keithrules22(temp,param);

	flag=0;
	if(y < leftloglike) flag=1;
	else if(y < rightloglike) flag=-1;

	while(K>0 && flag!=0) {
		if(runif()<0.5) {
			L = 2*L - R;
			if(L<thetaLeft[param])L=thetaLeft[param]+0.000001;
			temp[param] = L;
			if(y >=keithrules22(temp,param) && flag==1) flag=0; 
		}
		else {
			R = 2*R -L;
			if(R>thetaRight[param])R=thetaRight[param]-0.000001;
			temp[param] = R;
			if(y >=keithrules22(temp,param) && flag==-1) flag=0; 
		}
		K--;   
	}

	while(1){
		x = L + runif()*(R-L);
		if(x>thetaRight[param])x=thetaRight[param]-0.000001;
		if(x<thetaLeft[param])x=thetaLeft[param]+0.000001;
		temp[param] = x;
		if(keithrules22(temp,param)>y) break;
		if(theta[param] > x) L = x;
		else R = x;
	} 

	free(temp);
	return(x);

}
// End of Slice2


double keithrulesSIGMASQ(double XCHAIN[], double ZCHAIN[])
{
	int ii, j, jj;
	double sumsquared=0;
	double circledist, xkk;
/*
 */
	for(ii=0;ii<nrowX;ii++)
	{
		for(j=0;j<ncolX;j++)
		{
			if(X[ii*ncolX+j]> 0.0)
			{
		          if(XCOORDS[NS*ii] > -99)
		          {
  			    circledist=0.0;
		            for(jj=0;jj<NS;jj++)
		            {
			         circledist=circledist+pow((ZCHAIN[NS*j+jj]-XCHAIN[NS*ii+jj]),2.0);
		            }
		            circledist=sqrt(circledist);
			    sumsquared=sumsquared+pow((log(circledist)-log(X[ii*ncolX+j])),2.0);
			    xkk=xkk+1;
		          }
			}
		}
	}
//
	return sumsquared/xkk;
}
/*

    keithrulesX is Called by NELMIN to find X (individual) Coordinates
*/
double keithrulesX(double theta[])
{
	int i, j, jj, kk;
	double sumsquared=0;
	double circledist, circledisthat;
/*
 */
/*
 *
*/
	for(j=0;j<ncolX;j++)
	{
		circledist = XTEMP[j];
/*
CATCH MISSING DATA HERE
*/
		if(circledist > 0.0)
		{
			circledisthat=0.0;
			for(jj=0;jj<NS;jj++)
			{
				circledisthat = circledisthat+pow((theta[jj]-ZCOORDS[NS*j+jj]),2.0);
			}
			circledisthat=sqrt(circledisthat);
				   //catch logs of zero
			if(circledisthat<.0001)circledisthat=.001;
			sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
		}
	}
//	}
//  SIMPLE SUM OF SQUARED ERROR
//
	return sumsquared;
}
/*

   keithrulesZ is called by NELMIN to find Z (Stimuli Coordinates)
*/
double keithrulesZ(double theta[])
{
	int i, j, jj, kk, keithflag;
	double sumsquared=0;
	double circledist, circledisthat;
/*
 */
	for(j=0;j<nrowX;j++)
	{
		circledist = XTEMP[j];
/*
CATCH MISSING DATA HERE
*/
		if(circledist > 0.0)
		{
			keithflag=0;
			for(jj=0;jj<NS;jj++)
			{
				if(XCOORDS[NS*j+jj]< -98.0)keithflag=1;
			}
			if(keithflag==0)
			{
				circledisthat=0.0;
				for(jj=0;jj<NS;jj++)
				{
					circledisthat = circledisthat+pow((theta[jj]-XCOORDS[NS*j+jj]),2.0);
				}
				circledisthat=sqrt(circledisthat);
				   //catch logs of zero
				if(circledisthat<.0001)circledisthat=.001;
				sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
			}
		}
	}
//	}
//  SIMPLE SUM OF SQUARED ERROR
//
	return sumsquared;
}
/*
keithrules22 is called by sliceX -- used in slice sampler for the
individuals (X's)

*/
double keithrules22(double theta[], int param)
{
	int i, j, jj, kk;
	double sumsquared=0;
	double circledist, circledisthat;
/*
 */
/*
 *NOTE THAT THIS IS NDIM-1 IF SIGMA**2 IS BEING ESTIMATED!!!!
*/
	i=param/NS;
//	
	for(j=0;j<ncolX;j++)
	{
		circledist = XTEMP[j];
/*
CATCH MISSING DATA HERE
*/
		if(circledist > 0.0)
		{
			circledisthat=0.0;
			for(jj=0;jj<NS;jj++)
			{
//				circledisthat = circledisthat+pow((theta[jj]-ZCOORDS[NS*j+jj]),2.0);
				circledisthat = circledisthat+pow((theta[jj]-ZCHAIN[NS*j+jj]),2.0);
			}
			circledisthat=sqrt(circledisthat);
				   //catch logs of zero
			if(circledisthat<.0001)circledisthat=.001;
			sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
		}
	}
//	
//  SIMPLE SUM OF SQUARED LOG DIFFERENCES
//
//
	return -sumsquared;
}
/*

  keithrules33 is called by sliceZ which does the slice sampler for the
  Stimuli

*/
double keithrules33(double theta[], int param)
{
	int i, j, jj, kk, keithflag;
	double sumsquared=0;
	double circledist, circledisthat;
/*
 */
	for(j=0;j<nrowX;j++)
	{
		circledist = XTEMP[j];
/*
CATCH MISSING DATA HERE
*/
		if(circledist > 0.0)
		{
			circledisthat=0.0;
			keithflag=0;
			for(jj=0;jj<NS;jj++)
			{
				if(XCOORDS[NS*j+jj]< -98.0)keithflag=1;
			}
			if(keithflag==0)
			{
				for(jj=0;jj<NS;jj++)
				{
//					circledisthat = circledisthat+pow((theta[jj]-XCOORDS[NS*j+jj]),2.0);
					circledisthat = circledisthat+pow((theta[jj]-XCHAIN[NS*j+jj]),2.0);
				}
				circledisthat=sqrt(circledisthat);
				   //catch logs of zero
				if(circledisthat<.0001)circledisthat=.001;
				sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
			}
		}
	}
//	
//  SIMPLE SUM OF SQUARED ERROR
//
//
	return -sumsquared;
}


/******************************************************************************/

void nelmin ( double fn ( double x[] ), int n, double start[], double xmin[], 
  double *ynewlo, double reqmin, double step[], int konvge, int kcount, 
  int *icount, int *numres, int *ifault )

/******************************************************************************/
/*
  Purpose:

    NELMIN minimizes a function using the Nelder-Mead algorithm.

  Discussion:

    This routine seeks the minimum value of a user-specified function.

    Simplex function minimisation procedure due to Nelder+Mead(1965),
    as implemented by O'Neill(1971, Appl.Statist. 20, 338-45), with
    subsequent comments by Chambers+Ertel(1974, 23, 250-1), Benyon(1976,
    25, 97) and Hill(1978, 27, 380-2)

    The function to be minimized must be defined by a function of
    the form

      function fn ( x, f )
      double fn
      double x(*)

    and the name of this subroutine must be declared EXTERNAL in the
    calling routine and passed as the argument FN.

    This routine does not include a termination test using the
    fitting of a quadratic surface.

  Licensing:

    This code is distributed under the GNU LGPL license. 

  Modified:

    28 October 2010

  Author:

    Original FORTRAN77 version by R ONeill.
    C version by John Burkardt.

  Reference:

    John Nelder, Roger Mead,
    A simplex method for function minimization,
    Computer Journal,
    Volume 7, 1965, pages 308-313.

    R ONeill,
    Algorithm AS 47:
    Function Minimization Using a Simplex Procedure,
    Applied Statistics,
    Volume 20, Number 3, 1971, pages 338-345.

  Parameters:

    Input, double FN ( double x[] ), the name of the routine which evaluates
    the function to be minimized.

    Input, int N, the number of variables.

    Input/output, double START[N].  On input, a starting point
    for the iteration.  On output, this data may have been overwritten.

    Output, double XMIN[N], the coordinates of the point which
    is estimated to minimize the function.

    Output, double YNEWLO, the minimum value of the function.

    Input, double REQMIN, the terminating limit for the variance
    of function values.

    Input, double STEP[N], determines the size and shape of the
    initial simplex.  The relative magnitudes of its elements should reflect
    the units of the variables.

    Input, int KONVGE, the convergence check is carried out 
    every KONVGE iterations.

    Input, int KCOUNT, the maximum number of function 
    evaluations.

    Output, int *ICOUNT, the number of function evaluations 
    used.

    Output, int *NUMRES, the number of restarts.

    Output, int *IFAULT, error indicator.
    0, no errors detected.
    1, REQMIN, N, or KONVGE has an illegal value.
    2, iteration terminated because KCOUNT was exceeded without convergence.
*/
{
  double ccoeff = 0.5;
  double del;
  double dn;
  double dnn;
  double ecoeff = 2.0;
  /*
NOTE THAT EPS IS THE REAL STOPPING CRITERIA -- SEE PRINTING STATEMENTS
DOWN IN THE CODE!!!
*/
  double eps = 0.001;
  int i;
  int ihi;
  int ilo;
  int j;
  int jcount;
  int l;
  int nn;
  double *p;
  double *p2star;
  double *pbar;
  double *pstar;
  double rcoeff = 1.0;
  double rq;
  double x;
  double *y;
  double y2star;
  double ylo;
  double ystar;
  double z;
/*
  Check the input parameters.
*/
  if ( reqmin <= 0.0 )
  {
    *ifault = 1;
    return;
  }

  if ( n < 1 )
  {
    *ifault = 1;
    return;
  }

  if ( konvge < 1 )
  {
    *ifault = 1;
    return;
  }

  p = ( double * ) malloc ( n * ( n + 1 ) * sizeof ( double ) );
  pstar = ( double * ) malloc ( n * sizeof ( double ) );
  p2star = ( double * ) malloc ( n * sizeof ( double ) );
  pbar = ( double * ) malloc ( n * sizeof ( double ) );
  y = ( double * ) malloc ( ( n + 1 ) * sizeof ( double ) );

  *icount = 0;
  *numres = 0;

  jcount = konvge; 
  dn = ( double ) ( n );
  nn = n + 1;
  dnn = ( double ) ( nn );
  del = 1.0;
  rq = reqmin * dn;
/*
  Initial or restarted loop.
*/
  for ( ; ; )
  {
    for ( i = 0; i < n; i++ )
    { 
      p[i+n*n] = start[i];
    }
    y[n] = fn ( start );
    *icount = *icount + 1;

    for ( j = 0; j < n; j++ )
    {
      x = start[j];
      start[j] = start[j] + step[j] * del;
      for ( i = 0; i < n; i++ )
      {
        p[i+j*n] = start[i];
      }
      y[j] = fn ( start );
      *icount = *icount + 1;
      start[j] = x;
    }
/*                 
  The simplex construction is complete.
                    
  Find highest and lowest Y values.  YNEWLO = Y(IHI) indicates
  the vertex of the simplex to be replaced.
*/                
    ylo = y[0];
    ilo = 0;

    for ( i = 1; i < nn; i++ )
    {
      if ( y[i] < ylo )
      {
        ylo = y[i];
        ilo = i;
      }
    }
/*
  Inner loop.
*/
    for ( ; ; )
    {
      if ( kcount <= *icount )
      {
        break;
      }
      *ynewlo = y[0];
      ihi = 0;

      for ( i = 1; i < nn; i++ )
      {
        if ( *ynewlo < y[i] )
        {
          *ynewlo = y[i];
          ihi = i;
        }
      }
/*
  Calculate PBAR, the centroid of the simplex vertices
  excepting the vertex with Y value YNEWLO.
*/
      for ( i = 0; i < n; i++ )
      {
        z = 0.0;
        for ( j = 0; j < nn; j++ )
        { 
          z = z + p[i+j*n];
        }
        z = z - p[i+ihi*n];  
        pbar[i] = z / dn;
      }
/*
  Reflection through the centroid.
*/
      for ( i = 0; i < n; i++ )
      {
        pstar[i] = pbar[i] + rcoeff * ( pbar[i] - p[i+ihi*n] );
      }
      ystar = fn ( pstar );
      *icount = *icount + 1;
/*
  Successful reflection, so extension.
*/
      if ( ystar < ylo )
      {
        for ( i = 0; i < n; i++ )
        {
          p2star[i] = pbar[i] + ecoeff * ( pstar[i] - pbar[i] );
        }
        y2star = fn ( p2star );
        *icount = *icount + 1;
/*
  Check extension.
*/
        if ( ystar < y2star )
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = pstar[i];
          }
          y[ihi] = ystar;
        }
/*
  Retain extension or contraction.
*/
        else
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = p2star[i];
          }
          y[ihi] = y2star;
        }
      }
/*
  No extension.
*/
      else
      {
        l = 0;
        for ( i = 0; i < nn; i++ )
        {
          if ( ystar < y[i] )
          {
            l = l + 1;
          }
        }

        if ( 1 < l )
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = pstar[i];
          }
          y[ihi] = ystar;
        }
/*
  Contraction on the Y(IHI) side of the centroid.
*/
        else if ( l == 0 )
        {
          for ( i = 0; i < n; i++ )
          {
            p2star[i] = pbar[i] + ccoeff * ( p[i+ihi*n] - pbar[i] );
          }
          y2star = fn ( p2star );
          *icount = *icount + 1;
/*
  Contract the whole simplex.
*/
          if ( y[ihi] < y2star )
          {
            for ( j = 0; j < nn; j++ )
            {
              for ( i = 0; i < n; i++ )
              {
                p[i+j*n] = ( p[i+j*n] + p[i+ilo*n] ) * 0.5;
                xmin[i] = p[i+j*n];
              }
              y[j] = fn ( xmin );
              *icount = *icount + 1;
            }
            ylo = y[0];
            ilo = 0;

            for ( i = 1; i < nn; i++ )
            {
              if ( y[i] < ylo )
              {
                ylo = y[i];
                ilo = i;
              }
            }
            continue;
          }
/*
  Retain contraction.
*/
          else
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = p2star[i];
            }
            y[ihi] = y2star;
          }
        }
/*
  Contraction on the reflection side of the centroid.
*/
        else if ( l == 1 )
        {
          for ( i = 0; i < n; i++ )
          {
            p2star[i] = pbar[i] + ccoeff * ( pstar[i] - pbar[i] );
          }
          y2star = fn ( p2star );
          *icount = *icount + 1;
/*
  Retain reflection?
*/
          if ( y2star <= ystar )
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = p2star[i];
            }
            y[ihi] = y2star;
          }
          else
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = pstar[i];
            }
            y[ihi] = ystar;
          }
        }
      }
/*
  Check if YLO improved.
*/
      if ( y[ihi] < ylo )
      {
        ylo = y[ihi];
        ilo = ihi;
      }
      jcount = jcount - 1;

      if ( 0 < jcount )
      {
        continue;
      }
/*
  Check to see if minimum reached.
*/
      if ( *icount <= kcount )
      {
        jcount = konvge;

        z = 0.0;
        for ( i = 0; i < nn; i++ )
        {
          z = z + y[i];
        }
        x = z / dnn;

        z = 0.0;
        for ( i = 0; i < nn; i++ )
        {
          z = z + pow ( y[i] - x, 2 );
        }
        if ( z <= rq )
        {
          break;
        }
      }
    }
/*
  Factorial tests to check that YNEWLO is a local minimum.
*/
    for ( i = 0; i < n; i++ )
    {
      xmin[i] = p[i+ilo*n];
    }
    *ynewlo = y[ilo];

    if ( kcount < *icount )
    {
      *ifault = 2;
      break;
    }

    *ifault = 0;

    for ( i = 0; i < n; i++ )
    {
      del = step[i] * eps;
      xmin[i] = xmin[i] + del;
      z = fn ( xmin );
      *icount = *icount + 1;
//      fprintf(kp,"%10d %10d %10d %16.8f %16.8f %16.8f\n",i,*numres,*icount,*ynewlo,z,del);
      if ( z < *ynewlo )
      {
        *ifault = 2;
        break;
      }
      xmin[i] = xmin[i] - del - del;
      z = fn ( xmin );
      *icount = *icount + 1;
//      fprintf(kp,"%10d %10d %10d %16.8f %16.8f %16.8f\n",i,*numres,*icount,*ynewlo,z,(-del-del));
      if ( z < *ynewlo )
      {
        *ifault = 2;
        break;
      }
      xmin[i] = xmin[i] + del;
    }

    if ( *ifault == 0 )
    {
      break;
    }
/*
  Restart the procedure.
*/
    for ( i = 0; i < n; i++ )
    {
      start[i] = xmin[i];
    }
    del = eps;
    *numres = *numres + 1;
  }
  free ( p );
  free ( pstar );
  free ( p2star );
  free ( pbar );
  free ( y );

  return;
}
/*Singular Value Decomposition Subroutine

   */
void double_center(int kpnp, int kpnq, double *y, double *udc, double *lambdadc, double *vtdc) {
/*   
*/

	double *a, *b, *c, *work, *work2, *lambda2, *xcounts;
	double sumulv, svd_error_sum, svd_error_sum_2;
	int i, j, jj, ndebug;
	int  info = 12;
	int  lwork= kpnp*kpnp+kpnq*kpnq;
	int  lda,ldu,ldvt;

	a     = calloc( kpnp*kpnq, sizeof(double));
	b     = calloc( kpnp*kpnq, sizeof(double));
	c     = calloc( kpnp*kpnq, sizeof(double));
	work  = calloc( lwork, sizeof(double));
	work2  = calloc( lwork, sizeof(double));
	xcounts  = calloc( lwork, sizeof(double));
	lambda2  = calloc( lwork, sizeof(double));


	lda  = kpnp;
	ldu  = kpnp;
	ldvt = kpnq;
	for (i=0;i<lwork;i++){
		work[i] = 0;
		work2[i] = 0;
		xcounts[i] = 0;
	}

	fprintf(jp,"lwork=%i\n",lwork);

	for (j=0;j<kpnq;j++) {
		for (i=0;i<kpnp;i++) {
			a[(j*kpnp)+i] = y[(j*kpnp)+i];
		}
	}
/*
Double-Center the Matrix

*/
	for(i=0;i<kpnp;i++)
	{
		for(j=0;j<kpnq;j++)
		{
			if(a[i+j*kpnp] >= 0)
			{
				work2[i]=work2[i]+pow((a[i+j*kpnp]),2.0);
				xcounts[i]=xcounts[i]+1;
				work2[j+kpnp]=work2[j+kpnp]+pow((a[i+j*kpnp]),2.0);
				xcounts[j+kpnp]=xcounts[j+kpnp]+1;
			}
		}
	}

// Row Means
	svd_error_sum=0.0;
	sumulv=0.0;
	for(i=0;i<kpnp;i++)
	{
		work2[i]=work2[i]/xcounts[i];
		svd_error_sum=svd_error_sum+work2[i];
		sumulv=sumulv+1.0;    
	}
	svd_error_sum=svd_error_sum/sumulv;
// Column Means
	svd_error_sum_2=0.0;
	sumulv=0.0;
	for(j=0;j<kpnq;j++)
	{
		work2[j+kpnp]=work2[j+kpnp]/xcounts[j+kpnp];
		svd_error_sum_2=svd_error_sum_2+work2[j+kpnp];
		sumulv=sumulv+1.0;    
	}
	svd_error_sum_2=svd_error_sum_2/sumulv;
	for(i=0;i<kpnp;i++)
	{
		printf("dude %15.6f %15.6f %15.6f %15.6f\n",work2[i],work2[i+kpnp],xcounts[i],xcounts[i+kpnp]); 
	}
	fprintf(kp,"Matrix Means = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
	printf("Matrix Means = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
// Double Center
	for(i=0;i<kpnp;i++)
	{
		for(j=0;j<kpnq;j++)
		{
			if(a[i+j*kpnp] >= 0)
			{
				b[i+j*kpnp]=((pow((a[i+j*kpnp]),2.0)-work2[i]-work2[j+kpnp]+svd_error_sum)/(-2.0));
			}
			if(a[i+j*kpnp] < 0)
			{
				b[i+j*kpnp]=((svd_error_sum-work2[i]-work2[j+kpnp]+svd_error_sum)/(-2.0));
			}
			c[i+j*kpnp]=b[i+j*kpnp];
		}
	}
	for(i=0;i<kpnp;i++){
		fprintf(jp,"%10d",i);
		for(j=0;j<kpnq;j++)
		{
			fprintf(jp,"%12.6f",b[i+j*kpnp]);
		}
		fprintf(jp,"\n");
	}
/*

REMEMBER LAPACK IS FORTRAN SO THE COLUMNS ARE STACKED, IN C, THE ROWS
ARE STACKED.  IT DOES NOT MATTER HERE BECAUSE THE MATRICES ARE SYMMETRIC

*/
	dgesvd_("A","A", &kpnp, &kpnq, b, &lda, lambdadc,
		  udc, &ldu, vtdc, &ldvt, work, &lwork, &info);

	fprintf(kp,"Info = %i\n",info);
	printf("Info = %i\n",info);
	fprintf(kp,"Singular Values\n");
	printf("Singular Values\n");
	for(jj=0;jj<kpnq;jj++)
	{
		fprintf(kp,"%5d %16.6f\n",jj,lambdadc[jj]);
		printf("%5d %16.6f\n",jj,lambdadc[jj]);
	}
/*
  Do simple check of SVD

*/
	svd_error_sum=0.0;
	svd_error_sum_2=0.0;
	for (i=0;i<kpnp;i++)
	{
		for (jj=0;jj<kpnq;jj++)
		{
			sumulv=0.0;
			for (j=0;j<kpnq;j++)
			{
				sumulv+=udc[(j*kpnp)+i]*lambdadc[j]*vtdc[j+(jj*kpnq)];
			}
			svd_error_sum+=(c[i+(jj*kpnp)]-sumulv)*(c[i+(jj*kpnp)]-sumulv);
			svd_error_sum_2+=fabs(c[i+(jj*kpnp)]-sumulv);
		}
	}
	fprintf(kp,"SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
	printf("SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
	ndebug=1960;
	if(ndebug==1960)
	{
//		exit(EXIT_FAILURE);
	}
	for (j=0;j<kpnq;j++) {
		for (i=0;i<kpnp;i++) {
			a[(j*kpnp)+i] = c[(j*kpnp)+i];
		}
	}

/*
  Call Regular Eigenvalue-Eigenvector Routine as a Check

*/
	dsyev_("V","L", &kpnp, a, &lda, lambda2,
	       work, &lwork, &info);

	fprintf(kp,"Info = %i\n",info);
	printf("Info = %i\n",info);
	fprintf(kp,"Eigenvalues\n");
	printf("Eigenvalues\n");
	for(jj=0;jj<kpnq;jj++)
	{
		fprintf(kp,"%5d %16.6f\n",jj,lambda2[kpnq-1-jj]);
		printf("%5d %16.6f\n",jj,lambda2[kpnq-1-jj]);
	}
	free(work);
	free(work2);
	free(xcounts);
	free(a);
	free(b);
	free(c);
	free(lambda2);
}
/*********************************************************************
 *Calculate SSE and R-Square Between Coordinates and Input Matrix

 *********************************************************************
*/
void Least_Squares_Fit(double *XCOORDS, double *ZCOORDS) {
	int i, j, jj, kk, kkk;
	double sumsquared=0;
	double circledist, circledisthat;
	double aa, bb, cc, dd, ee, rsquarexz;
	double aaa, bbb, ccc;
/*
 */
	kk=0;
	aa=0;
	bb=0;
	cc=0;
	dd=0;
	ee=0;
	
	for(i=0;i<nrowX;i++)
	{
		kkk=0;
		for(j=0;j<ncolX;j++)
		{
			if(X[i*ncolX+j] < 0.0)kkk=kkk+1;
		}
	  for(j=0;j<ncolX;j++)
	  {
		  if(kkk <= 7)
		  {
		   circledist = X[i*ncolX+j];
/*
CATCH MISSING DATA HERE
*/
		   if(circledist > 0.0)
		   {
			circledisthat=0.0;
			for(jj=0;jj<NS;jj++)
			{
				circledisthat = circledisthat+pow((XCOORDS[NS*i+jj]-ZCOORDS[NS*j+jj]),2.0);
			}
			circledisthat=sqrt(circledisthat);
			   //catch logs of zero
			if(circledisthat<.0001)circledisthat=.001;
			sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
			kk=kk+1;
			aa=aa+circledisthat;
			bb=bb+circledist;
			cc=cc+circledisthat*circledisthat;
			dd=dd+circledist*circledist;
			ee=ee+circledisthat*circledist;
//			fprintf(jp,"%6d %6d %12d %14f %14f %14f\n",i,j,kk,circledist,circledisthat,sumsquared);
		   }
		  }
	   }
	}
	aaa=((double)kk)*ee-aa*bb;
	bbb=((double)kk)*cc-aa*aa;
	ccc=((double)kk)*dd-bb*bb;
	rsquarexz=(aaa*aaa)/(bbb*ccc);
	printf(" SSE and R-Square %14f %14f\n",sumsquared, rsquarexz);
	fprintf(jp," SSE and R-Square %14f %14f\n",sumsquared, rsquarexz);

}
/*Singular Value Decomposition Subroutine

   */
void xsvd(int kpnp, int kpnq, double *y, double *u, double *lambda, double *vt) {
/*   
*/

	double *a, *work;
	double sumulv, svd_error_sum, svd_error_sum_2;
	int i, j, jj;
	int  info = 12;
	int  lwork= 500*(kpnp*kpnp+kpnq*kpnq);
	int  lda,ldu,ldvt,kpnqm1;

	a     = calloc( kpnp*kpnq, sizeof(double));
	work  = calloc( lwork, sizeof(double));
	lambda  = calloc( lwork, sizeof(double));

// kpnp=12 = ncolX
// kpnq=2 = NS	
	lda  = kpnp;
	ldu  = kpnp;
	ldvt = kpnq;
	kpnqm1 = kpnq-1;
	for (i=0;i<lwork;i++){
		work[i] = 0;
	}

	fprintf(jp,"lwork=%i\n",lwork);

//
// In C the Matrix is stacked by Rows -- In LAPACK FORTRAN Routines the
//   Matrices are stacked by Columns
//
	for (j=0;j<kpnq;j++) {
		for (i=0;i<kpnp;i++) {
			a[(j*kpnp)+i] = y[(i*kpnq)+j];
		}
	}

	for(j=0;j<kpnp;j++)
	{
		printf("%5d ",j);
		fprintf(jp,"%5d ",j);
		for(jj=0;jj<kpnq;jj++)
		{
			printf("%15.6f",a[(jj*kpnp)+j]);
			fprintf(jp,"%15.6f",a[(jj*kpnq)+j]);
		}
		printf("\n");
		fprintf(jp,"\n");
	}

	dgesvd_("A","A", &kpnp, &kpnq, a, &lda, lambda,
		  u, &ldu, vt, &ldvt, work, &lwork, &info);

	fprintf(kp,"Info = %i\n",info);
	printf("Info = %i\n",info);
	fprintf(kp,"Singular Values\n");
	printf("Singular Values\n");
	for(jj=0;jj<kpnq;jj++)
	{
		fprintf(kp,"%5d %16.6f\n",jj,lambda[jj]);
		printf("%5d %16.6f\n",jj,lambda[jj]);
	}
/*
  Do simple check of SVD

*/
	svd_error_sum=0.0;
	svd_error_sum_2=0.0;
//
//  REMEMBER THAT y COMES IN WITH C-STYLE ROW STACKING --
//  LAPACK IS FORTRAN AND IS USING COLUMN STACKING
//
	for (i=0;i<kpnp;i++)
	{
		for (jj=0;jj<kpnq;jj++)
		{
			sumulv=0.0;
			for (j=0;j<kpnq;j++)
			{
				sumulv=sumulv+u[(j*kpnp)+i]*lambda[j]*vt[j+(jj*kpnq)];
			}
			svd_error_sum=svd_error_sum+pow((y[(i*kpnq)+jj]-sumulv),2.0);
			svd_error_sum_2=svd_error_sum_2+fabs(y[(i*kpnq)+jj]-sumulv);
		}
	}
	fprintf(kp,"SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
	printf("SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);

/*
  Call QR Routine as a Check
*/

	free(work);
	free(a);
	free(lambda);
}

/*Singular Value Decomposition Subroutine

   */
//
//  NOTE THAT y IS STACKED BY ROW IN C BUT IT MUST BE STACKED BY COLUMN
//     IN THE FORTRAN CALL
//
void xsvd2(int kpnp, int kpnq, double *y, double *rmatrix) {
/*   
*/

	double *a, *b, *work, *tau, *XprimeX, *XprimeX2, *XXinv, *XXinvX, *ACHECK, *coef, *ACHECK2;
	double saveH, sum, sum2;
	double alpha = 1.0, beta=0.0;
	int i, j, jj, jjj, ipiv[kpnq];
	int  info = 12;
	int  lwork= 500*(kpnp*kpnp+kpnq*kpnq);
	int  lda,ldu,ldvt,kpnqm1,nrow,ncol;
	char trans = 't', notrans ='n';

	a     = calloc( (kpnp+1)*kpnq, sizeof(double));
	b     = calloc( (kpnp+1)*kpnq, sizeof(double));
	work  = calloc( lwork, sizeof(double));
	tau  = calloc( lwork, sizeof(double));
	XprimeX  = calloc( kpnq*kpnq, sizeof(double));
	XprimeX2 = calloc( kpnq*kpnq, sizeof(double));
	XXinv = calloc( kpnq*kpnq, sizeof(double));
	XXinvX = calloc (kpnq*(kpnp+1), sizeof(double));
	ACHECK = calloc (kpnq*kpnq, sizeof(double));
	ACHECK2 = calloc ((kpnp+1)*kpnq, sizeof(double));
	coef = calloc (kpnq*kpnq, sizeof(double));
//
	lda  = kpnp+1;
	nrow = kpnp+1;
	ncol = kpnq;
	ldu  = kpnp;
	ldvt = kpnq;
	kpnqm1 = kpnq-1;
	for (i=0;i<lwork;i++){
		work[i] = 0;
	}

	printf("lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
	fprintf(jp,"lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
// Storage by Column FORTRAN Style -- y is storage by row
	for (i=0;i<(nrow);i++) {
		for (j=0;j<kpnq;j++) {
			a[(j*(nrow))+i] = y[(i*kpnq)+j];
			b[(j*(nrow))+i] = y[(i*kpnq)+j];
		}
	}

	for(j=0;j<(nrow);j++)
	{
		printf("%5d ",j);
		fprintf(jp,"%5d ",j);
		for(jj=0;jj<kpnq;jj++)
		{
			printf("%15.6f",a[jj*(nrow)+j]);
			fprintf(jp,"%15.6f",a[jj*(nrow)+j]);
		}
		printf("\n");
		fprintf(jp,"\n");
	}

//  Call QR Routine as a Check


//  Pass it the Matrix


	for(j=0;j<(nrow);j++)
	{
		printf("%5d ",j);
		fprintf(jp,"%5d ",j);

		for(jj=0;jj<kpnq;jj++)

		{
			printf("%15.6f",a[jj*(nrow)+j]);
			fprintf(jp,"%15.6f",a[jj*(nrow)+j]);

		}
		printf("\n");
		fprintf(jp,"\n");
	}
	for (i=0;i<lwork;i++){
		work[i] = 0;
		tau[i] = 0;
	}
	printf("kpnp = %i\n",kpnp);
	printf("kpnq = %i\n",kpnq);
	printf("lda  = %i\n",lda);
//	printf("A = [");
//	for (i=0;i<((nrow)*kpnq);i++) printf("%6.4f,",a[i]);
//	printf("]\n\n");

	dgerqf_(&kpnp, &kpnq, a, &lda, tau, work, &lwork, &info);
//	dgeqrf_(&kpnp, &kpnq, a, &lda, tau, work, &lwork, &info);
	fprintf(jp,"QR Info = %i %15.6f\n",info, work[0]);
//	printf("QR Info = %i %15.6f\n",info, work[0]);
//	printf("After DGERQF_...\n");
//	fprintf(jp,"A = [");
//	printf("A = [");
//	for (i=0;i<((nrow)*kpnq);i++) printf("%6.4f,",a[i]);
//	for (i=0;i<((nrow)*kpnq);i++) fprintf(jp,"%6.4f,",a[i]);
//	fprintf(jp,"]\n\n");
//	printf("]\n\n");
	saveH=a[kpnp-1];	
//	a[kpnp-2]=0.0; // Not real sure why, but this happens in the Fortran code
//	a[kpnp-1]=0.0; // Not real sure why, but this happens in the Fortran code
//	a[22]=0.0; // Not real sure why, but this happens in the Fortran code
	for(j=1;j<(kpnq+1);j++)
	{
		for(jj=0;jj<(kpnq+1-j);jj++)
		{
			a[(j*nrow)-1-jj]=0.0;
		}
	}
//	
	for(j=0;j<(nrow);j++)
	{
		printf("%5d ",j);
		fprintf(jp,"%5d ",j);
		for(jj=0;jj<kpnq;jj++)
		{
			printf("%15.6f",a[jj*(nrow)+j]);
			fprintf(jp,"%15.6f",a[jj*(nrow)+j]);
			rmatrix[(j*kpnq)+jj]=a[(jj*(nrow))+j];
		}
		printf("\n");
		fprintf(jp,"\n");
	}
	printf("\nTau::\n");
	for(jj=0;jj<kpnq;jj++)
	{
		printf("%15.6f",tau[jj]);
		fprintf(jp,"%15.6f",tau[jj]);
	}
	printf("%15.6f",saveH);
	fprintf(jp,"%15.6f",saveH);
	printf("\n");
	fprintf(jp,"\n");
//	
//	Computes the matrix operation: alpha*(A_transpose*B) + beta*C,
//	where A, B, and C are matrices
//	and alpha and beta are scalers. Note that if A=B, alpha=1, and
//      beta=0, then this routine returns A_transpose*A
//
//      SUBROUTINE DGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
//
//  COMPUTES (t(R)*R), alpha=1, beta=0
//
	dgemm_(&trans,&notrans,&ncol,&ncol,&nrow,&alpha,a,&nrow,a,&nrow,&beta,XprimeX,&ncol);
	printf("\n\nX'X = \n");
	for(i=0;i<ncol;i++)
	{
		for(j=0;j<ncol;j++)
		{
			printf("%12.6f",XprimeX[(i*ncol)+j]);
		}
		printf("\n");
	}
	for (i=0;i<((kpnq)*kpnq);i++) XprimeX2[i]=XprimeX[i];
//
//	LAPACK Routine Used in OLS.C -- Solves the linear equation
//	problem of A * X = B.
//      Note that if B is set equal to the identity matrix, then the
//      routine returns A_inverse
//
	for(j=0;j<kpnq;j++)
	{
		for(jj=0;jj<kpnq;jj++)
		{
			XXinv[(j*ncol)+jj]=0.0;
			if(j==jj)XXinv[(j*ncol)+jj]=1.0;
		}
	}
//	
//	SUBROUTINE DGESV( N, NRHS, A, LDA, IPIV, B, LDB, INFO )
//
	dgesv_(&ncol,&ncol,XprimeX,&ncol,ipiv,XXinv,&ncol,&info);
	fprintf(jp,"\n\n(X'X)-1 = \n");
	printf("\n\n(X'X)-1 = \n");
	for(i=0;i<ncol;i++)
	{
		for(j=0;j<ncol;j++)
		{
			fprintf(jp,"%12.6f",XXinv[(i*ncol)+j]);
			printf("%12.6f",XXinv[(i*ncol)+j]);
		}
		fprintf(jp,"\n");
		printf("\n");
	}
// Check inverse
	for(i=0;i<ncol;i++)
	{
		for(jj=0;jj<ncol;jj++)
		{
			sum=0.0;
			for(j=0;j<ncol;j++)
			{
				sum=sum+XXinv[(i*ncol)+j]*XprimeX2[(jj*ncol)+j];
			}
			ACHECK[(i*ncol)+jj]=sum;
		}
	}
//	printf("\n");
//	for (i=0;i<((kpnq)*kpnq);i++) printf("%6.4f,",XXinv[i]);
//	printf("\n");
//	for (i=0;i<((kpnq)*kpnq);i++) printf("%6.4f,",XprimeX2[i]);
	printf("\n\n(X'X)-1*(X'X) = \n");
	fprintf(jp,"\n\n(X'X)-1*(X'X) = \n");
	for(i=0;i<ncol;i++)
	{
		for(j=0;j<ncol;j++)
		{
			printf("%12.6f",ACHECK[(i*ncol)+j]);
			fprintf(jp,"%12.6f",ACHECK[(i*ncol)+j]);
		}
		printf("\n");
		fprintf(jp,"\n");
	}
//XXinv is 2x2
//X' is 2x5
//X is 5x2

//
//      SUBROUTINE DGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
//
//  COMPUTES [(t(R)*R)-1]*R] returned in XXinvX, alpha=1, beta=0
//
	dgemm_(&notrans,&trans,&ncol,&nrow,&ncol,&alpha,XXinv,&ncol,a,&nrow,&beta,XXinvX,&ncol);
// COMPUTES  [(t(R)*R)-1]*R]*X = Q where Q is returned in coef
	dgemm_(&notrans,&notrans,&ncol,&ncol,&nrow,&alpha,XXinvX,&ncol,b,&nrow,&beta,coef,&ncol);
	printf("\nQ Matrix\n");
	fprintf(jp,"\nQ Matrix\n");
	for (i=0;i<kpnq;i++)
	{
		for(j=0;j<kpnq;j++)
		{
			printf("%6.4f,",coef[(i*ncol)+j]);
			fprintf(jp,"%6.4f,",coef[(i*ncol)+j]);
		}
		fprintf(jp,"\n");
		printf("\n");
	}
//
// Check Result
	sum2=0.0;
	for(i=0;i<nrow;i++)
	{
		for(jj=0;jj<ncol;jj++)
		{
			sum=0.0;
			for(j=0;j<ncol;j++)
			{
				sum=sum+a[i+(nrow*j)]*coef[(jj*ncol)+j];
			}
			ACHECK2[i+(nrow*jj)]=sum;
			sum2=sum2+pow((sum-b[i+(nrow*jj)]),2.0);
		}
		printf("\n");
		for (jjj=0;jjj<ncol;jjj++) printf("%8.4f,",ACHECK2[i+(nrow*jjj)]);
	}
	fprintf(jp,"\n Accuracy Check: %12.6f\n",sum2);
	printf("\n Accuracy Check: %12.6f\n",sum2);
	free(a);
	free(b);
	free(work);
	free(tau);
	free(XprimeX);
	free(XprimeX2);
	free(XXinv);
	free(XXinvX);
	free(ACHECK);
	free(ACHECK2);
	free(coef);
}

/******************************************************************************/

void timestamp ( void )

/******************************************************************************/
/*
  Purpose:

    TIMESTAMP prints the current YMDHMS date as a time stamp.

  Example:

    31 May 2001 09:45:54 AM

  Licensing:

    This code is distributed under the GNU LGPL license. 

  Modified:

    24 September 2003

  Author:

    John Burkardt

  Parameters:

    None
*/
{
# define TIME_SIZE 40

  static char time_buffer[TIME_SIZE];
  const struct tm *tm;
  size_t len;
  time_t now;

  now = time ( NULL );
  tm = localtime ( &now );

  len = strftime ( time_buffer, TIME_SIZE, "%d %B %Y %I:%M:%S %p", tm );

  printf ( "%s\n", time_buffer );
  fprintf (jp, "%s\n", time_buffer );
  fprintf (kp, "%s\n", time_buffer );
  fprintf (lp, "%s\n", time_buffer );
  fprintf (mp, "%s\n", time_buffer );

  return;
# undef TIME_SIZE
}
//  ORTHOGONAL PROCRUSTES ROTATION SUBROUTINE
//
//  NOTE THAT y IS STACKED BY ROW IN C BUT IT MUST BE STACKED BY COLUMN
//     IN THE FORTRAN CALL
//L(T) = tr(A - BT)(A - BT)'
//
//   The solution is:

//   T = VU' where

//   A'B = ULV'
//
void xsvdrotate(int kpnp, int kpnq, double *y, double *yrotate, double *rmatrix, double *ATRUE, double *BTRUE) {
/*   
*/

	double *a, *b, *work, *XprimeX, *XXinv;
	double *u, *lambda, *vt, *ydummy;
	double sum, sum2, svd_error_sum, svd_error_sum_2, sumulv, timea, timeb, timec, timed;
	int i, j, jj, jjj;
	int  info = 12;
//	int  lwork= 500*(kpnp*kpnp+kpnq*kpnq);
	int  lwork= 100*(kpnp+kpnq);
	int  lda,ldu,ldvt,nrow,ncol;

	a     = calloc( (kpnp)*kpnq, sizeof(double));
	b     = calloc( (kpnp)*kpnq, sizeof(double));
	work  = calloc( lwork, sizeof(double));
	XprimeX  = calloc( kpnq*kpnq, sizeof(double));
	ydummy  = calloc( kpnq*kpnq, sizeof(double));
	XXinv = calloc( kpnq*kpnq, sizeof(double));
	u      = (double *) malloc (kpnq*kpnq*sizeof(double));
	lambda = (double *) malloc (kpnq*kpnq*sizeof(double));
	vt     = (double *) malloc (kpnq*kpnq*sizeof(double));
//
	lda  = kpnq;
	nrow = kpnp;
	ncol = kpnq;
	ldu  = kpnq;
	ldvt = kpnq;
//
//	timea = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call At Entry %12.3f seconds\n", timea);
//

	for (i=0;i<lwork;i++){
		work[i] = 0;
	}
	for(i=0;i<nrow*ncol;i++)
	{
		a[i]=ATRUE[i];
		b[i]=BTRUE[i];
	}
//	printf("lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
//	fprintf(jp,"lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
// Storage by Column FORTRAN Style -- y is storage by row
	for (i=0;i<(nrow);i++) {
		for (j=0;j<kpnq;j++) {
			a[(j*(nrow))+i] = y[(i*kpnq)+j];
			b[(j*(nrow))+i] = yrotate[(i*kpnq)+j];
		}
	}
//
//	timed = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call FORTRAN %12.3f seconds\n", timed);
//
	sum2=0.0;
//	for (i=0;i<(nrow*ncol);i++)
//	{
//		sum2=sum2+pow((y[i]-yrotate[i]),2.0);
//	}
// Calculate A'B -- store in XprimeX FORTRAN column stacked style
	for(j=0;j<(ncol);j++)
	{
		for(jj=0;jj<(ncol);jj++)
		{
			sum=0.0;
			for(i=0;i<nrow;i++)
			{
				sum=sum+a[j*(nrow)+i]*b[jj*(nrow)+i];
			}
			XprimeX[jj*ncol+j]=sum;
		}
	}


//  Pass it the Matrix

//
//	timeb = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call After A'B %12.3f seconds\n", timeb);
//

	for (i=0;i<(ncol*ncol);i++){
		ydummy[i]=XprimeX[i];
	}
//	printf("kpnp = %i\n",kpnp);
//	printf("kpnq = %i\n",kpnq);
//	printf("lda  = %i\n",lda);
	dgesvd_("A","A", &kpnq, &kpnq, XprimeX, &lda, lambda,
		  u, &ldu, vt, &ldvt, work, &lwork, &info);

/*
  Do simple check of SVD

*/
	svd_error_sum=0.0;
	svd_error_sum_2=0.0;
	for (i=0;i<kpnq;i++)
	{
		for (jj=0;jj<kpnq;jj++)
		{
			sumulv=0.0;
			for (j=0;j<kpnq;j++)
			{
				sumulv+=u[(j*kpnq)+i]*lambda[j]*vt[j+(jj*kpnq)];
			}
			svd_error_sum+=(ydummy[i+(jj*kpnq)]-sumulv)*(ydummy[i+(jj*kpnq)]-sumulv);
			svd_error_sum_2+=fabs(ydummy[i+(jj*kpnq)]-sumulv);
		}
	}
//	fprintf(jp,"SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
//	printf("SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
//
//	timec = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call after SVD %12.3f seconds\n", timec);
//
//
//  Write out U and vt as checks -- U and V' are stored by column,
//  FORTRAN style

//   The solution is:

//   T = VU' where T=rmatrix
//     Put into rmatrix[.] in C style stacked by rows
//
	for (i=0;i<kpnq;i++)
	{
		for (j=0;j<kpnq;j++)
		{
			sum=0.0;
			for (jj=0;jj<kpnq;jj++)
			{
				sum=sum+vt[jj+(j*kpnq)]*u[i+(jj*kpnq)];
			}
//			printf("%5d %5d %10.5f \n",i,j,sum);
			rmatrix[i+(j*kpnq)]=sum;
		}
	}
//
// Rotate B (ZCOORDS2 == yrotate)
//
	for (i=0;i<(nrow);i++) {
		for (j=0;j<kpnq;j++)
		{
			sum=0.0;
			for (jj=0;jj<kpnq;jj++) {
				sum=sum+yrotate[(i*kpnq)+jj]*rmatrix[(jj*kpnq)+j];
			}
			b[(i*kpnq)+j]=sum;
		}
//		printf("%5d %10.5f %10.5f %10.5f %10.5f \n",i,yrotate[(i*kpnq)+0],yrotate[(i*kpnq)+1],b[(i*kpnq)+0],b[(i*kpnq)+1]);
//		fprintf(jp,"%5d %10.5f %10.5f %10.5f %10.5f \n",i,yrotate[(i*kpnq)+0],yrotate[(i*kpnq)+1],b[(i*kpnq)+0],b[(i*kpnq)+1]);
	}
//
// Transfer into yrotate for passing back
//
	for (i=0;i<(nrow*kpnq);i++)
	{
		yrotate[i]=b[i];
	}
	sum=0.0;
//	for (i=0;i<(nrow*ncol);i++)
//	{
//		sum=sum+pow((y[i]-yrotate[i]),2.0);
////		sum=sum+pow((y[i]-b[i]),2.0);
//	}
//	printf("\nSSE Before and After Rotation %10.5f %10.5f\n",sum2,sum);
//	fprintf(jp,"\nSSE Before and After Rotation %10.5f %10.5f\n",sum2,sum);
//
	free(a);
	free(b);
	free(work);
	free(XprimeX);
	free(ydummy);
	free(XXinv);
	free(u);
	free(lambda);
	free(vt);
}
//  ORTHOGONAL PROCRUSTES ROTATION SUBROUTINE
//
//  NOTE THAT y IS STACKED BY ROW IN C BUT IT MUST BE STACKED BY COLUMN
//     IN THE FORTRAN CALL
//L(T) = tr(A - BT)(A - BT)'
//
//   The solution is:

//   T = VU' where

//   A'B = ULV'
//
void xsvdrotate2(int kpnp, int kpnq, double *y, double *yrotate, double *rmatrix, double *ATRUE, double *BTRUE) {
/*   
*/

	double *a, *b, *work, *XprimeX, *XXinv;
	double *u, *lambda, *vt, *ydummy;
	double sum, sum2, svd_error_sum, svd_error_sum_2, sumulv, timea, timeb, timec, timed;
	int i, j, jj, jjj;
	int  info = 12;
//	int  lwork= 500*(kpnp*kpnp+kpnq*kpnq);
	int  lwork= 100*(kpnp+kpnq);
	int  lda,ldu,ldvt,nrow,ncol;

	a     = calloc( (kpnp)*kpnq, sizeof(double));
	b     = calloc( (kpnp)*kpnq, sizeof(double));
	work  = calloc( lwork, sizeof(double));
	XprimeX  = calloc( kpnq*kpnq, sizeof(double));
	ydummy  = calloc( kpnq*kpnq, sizeof(double));
	XXinv = calloc( kpnq*kpnq, sizeof(double));
	u      = (double *) malloc (kpnq*kpnq*sizeof(double));
	lambda = (double *) malloc (kpnq*kpnq*sizeof(double));
	vt     = (double *) malloc (kpnq*kpnq*sizeof(double));
//
	lda  = kpnq;
	nrow = kpnp;
	ncol = kpnq;
	ldu  = kpnq;
	ldvt = kpnq;
//
//	timea = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call At Entry %12.3f seconds\n", timea);
//

	for (i=0;i<lwork;i++){
		work[i] = 0;
	}
	for(i=0;i<nrow*ncol;i++)
	{
		a[i]=ATRUE[i];
		b[i]=BTRUE[i];
	}
//	printf("lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
//	fprintf(jp,"lwork=%5d %5d %5d %5d\n",lwork,kpnp,kpnq,lda);
// Storage by Column FORTRAN Style -- y is storage by row
	for (i=0;i<(nrow);i++) {
		for (j=0;j<kpnq;j++) {
			a[(j*(nrow))+i] = y[(i*kpnq)+j];
			b[(j*(nrow))+i] = yrotate[(i*kpnq)+j];
		}
	}
//
//	timed = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call FORTRAN %12.3f seconds\n", timed);
//
	sum2=0.0;
//	for (i=0;i<(nrow*ncol);i++)
//	{
//		sum2=sum2+pow((y[i]-yrotate[i]),2.0);
//	}
// Calculate A'B -- store in XprimeX FORTRAN column stacked style
	for(j=0;j<(ncol);j++)
	{
		for(jj=0;jj<(ncol);jj++)
		{
			sum=0.0;
			for(i=0;i<nrow;i++)
			{
				sum=sum+a[j*(nrow)+i]*b[jj*(nrow)+i];
			}
			XprimeX[jj*ncol+j]=sum;
		}
	}


//  Pass it the Matrix

//
//	timeb = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call After A'B %12.3f seconds\n", timeb);
//

	for (i=0;i<(ncol*ncol);i++){
		ydummy[i]=XprimeX[i];
	}
//	printf("kpnp = %i\n",kpnp);
//	printf("kpnq = %i\n",kpnq);
//	printf("lda  = %i\n",lda);
	dgesvd_("A","A", &kpnq, &kpnq, XprimeX, &lda, lambda,
		  u, &ldu, vt, &ldvt, work, &lwork, &info);

/*
  Do simple check of SVD

*/
	svd_error_sum=0.0;
	svd_error_sum_2=0.0;
	for (i=0;i<kpnq;i++)
	{
		for (jj=0;jj<kpnq;jj++)
		{
			sumulv=0.0;
			for (j=0;j<kpnq;j++)
			{
				sumulv+=u[(j*kpnq)+i]*lambda[j]*vt[j+(jj*kpnq)];
			}
			svd_error_sum+=(ydummy[i+(jj*kpnq)]-sumulv)*(ydummy[i+(jj*kpnq)]-sumulv);
			svd_error_sum_2+=fabs(ydummy[i+(jj*kpnq)]-sumulv);
		}
	}
//	fprintf(jp,"SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
//	printf("SVD Error Check = %12.7g %12.7g\n",svd_error_sum,svd_error_sum_2);
//
//	timec = ( ((double) clock()) / CLOCKS_PER_SEC);
//	printf("Elapsed time Rotation Call after SVD %12.3f seconds\n", timec);
//
//
//  Write out U and vt as checks -- U and V' are stored by column,
//  FORTRAN style

//   The solution is:

//   T = VU' where T=rmatrix
//     Put into rmatrix[.] in C style stacked by rows
//
	for (i=0;i<kpnq;i++)
	{
		for (j=0;j<kpnq;j++)
		{
			sum=0.0;
			for (jj=0;jj<kpnq;jj++)
			{
				sum=sum+vt[jj+(j*kpnq)]*u[i+(jj*kpnq)];
			}
//			printf("%5d %5d %10.5f \n",i,j,sum);
//			rmatrix[i+(j*kpnq)]=sum;
		}
	}
//
// Rotate B (ZCOORDS2 == yrotate)
//
	for (i=0;i<(nrow);i++) {
		for (j=0;j<kpnq;j++)
		{
			sum=0.0;
			for (jj=0;jj<kpnq;jj++) {
				sum=sum+yrotate[(i*kpnq)+jj]*rmatrix[(jj*kpnq)+j];
			}
			b[(i*kpnq)+j]=sum;
		}
//		printf("%5d %10.5f %10.5f %10.5f %10.5f \n",i,yrotate[(i*kpnq)+0],yrotate[(i*kpnq)+1],b[(i*kpnq)+0],b[(i*kpnq)+1]);
//		fprintf(jp,"%5d %10.5f %10.5f %10.5f %10.5f \n",i,yrotate[(i*kpnq)+0],yrotate[(i*kpnq)+1],b[(i*kpnq)+0],b[(i*kpnq)+1]);
	}
//
// Transfer into yrotate for passing back
//
	for (i=0;i<(nrow*kpnq);i++)
	{
		yrotate[i]=b[i];
	}
	sum=0.0;
//	for (i=0;i<(nrow*ncol);i++)
//	{
//		sum=sum+pow((y[i]-yrotate[i]),2.0);
////		sum=sum+pow((y[i]-b[i]),2.0);
//	}
//	printf("\nSSE Before and After Rotation %10.5f %10.5f\n",sum2,sum);
//	fprintf(jp,"\nSSE Before and After Rotation %10.5f %10.5f\n",sum2,sum);
//
	free(a);
	free(b);
	free(work);
	free(XprimeX);
	free(ydummy);
	free(XXinv);
	free(u);
	free(lambda);
	free(vt);
}

//
static lbfgsfloatval_t evaluate(
				void *instance,
				const lbfgsfloatval_t *x,
				lbfgsfloatval_t *g,
				const int n,
				const lbfgsfloatval_t step
			       )
{
	int i, j, jj, kk, istop, idebug, keithflag;
	lbfgsfloatval_t fx = 0.0;
	double sumsquared=0;
	double circledist, circledisthat;
	double PI=3.141592653589793;
	double sumsquareddstar=0;
	double *ZCOORDSD, *XCOORDSD, *GG;
	ZCOORDSD     = calloc( ((nrowX+ncolX)*NS), sizeof(double));
	XCOORDSD     = calloc( ((nrowX+ncolX)*NS), sizeof(double));
	GG     = calloc(((nrowX+ncolX)*NS), sizeof(double));
/*
 */
/*
 *
*/
	kk=0;
//
	for(j=0;j<(NS*ncolX);j++)
	{
		GG[j]=0.0;
		if(CONSTRAINTS[j]<1.0){
			ZCOORDSD[j]=0.0;
		}
		if(CONSTRAINTS[j]>0.0){
			ZCOORDSD[j]=x[kk];
			kk=kk+1;
		}
	}
	for(j=0;j<(NS*nrowX);j++)
	{
		GG[j+(NS*ncolX)]=0.0;
		XCOORDSD[j]=0.0;
		if(XCOORDS[j]>-99.0){
			XCOORDSD[j]=x[kk];
			kk=kk+1;
		}
		
	}
//
	for(i=0;i<nrowX;i++)
	{
		for(j=0;j<ncolX;j++)
		{
			circledist = X[i*ncolX+j];
/*
CATCH MISSING DATA HERE
*/
			if(circledist > 0.0)
			{
				keithflag=0;
				for(jj=0;jj<NS;jj++)
				{
					if(XCOORDS[NS*i+jj]< -98.0)keithflag=1;
				}
				if(keithflag==0)
				{
 				   circledisthat=0.0;
				   for(jj=0;jj<NS;jj++)
				   {
					circledisthat = circledisthat+pow((XCOORDSD[NS*i+jj]-ZCOORDSD[NS*j+jj]),2.0);
				   }
				   circledisthat=sqrt(circledisthat);
				   //catch logs of zero
				   if(circledisthat<.0001)circledisthat=.001;
				   sumsquared=sumsquared+pow((log(circledist)-log(circledisthat)),2.0);
/* DERIVATIVES FOR RESPONDENTS
 * */
				   for(jj=0;jj<NS;jj++)
				   {
					   GG[NS*i+jj+NS*ncolX]=GG[NS*i+jj+NS*ncolX]-(log(circledist)-log(circledisthat))*(1.0/(pow(circledisthat,2.0)))*(XCOORDSD[NS*i+jj]-ZCOORDSD[NS*j+jj]);
				   }
/* DERIVATIVES FOR STIMULI
 * */
				   for(jj=0;jj<NS;jj++)
				   {
					   GG[NS*j+jj]=GG[NS*j+jj]+(log(circledist)-log(circledisthat))*(1.0/(pow(circledisthat,2.0)))*(XCOORDSD[NS*i+jj]-ZCOORDSD[NS*j+jj]);
				   }
				}
			}
		}
	}
	fx=sumsquared;
//    fprintf(jp,"LOSS FUNCTION L-BFGS %5d %5d %5d %5d %20.6f\n",n,N,nrowX,ncolX,fx);
//    printf("LOSS FUNCTION L-BFGS %5d %5d %5d %5d %20.6f\n",n,N,nrowX,ncolX,fx);
//
//  TRANSFER INTO GRADIENT VECTOR
//
	kk=0;
	for(j=0;j<(ncolX*NS);j++)
	{
		if(CONSTRAINTS[j]>0.0){
			g[kk]=GG[j];
			kk=kk+1;
		}
	}
	for(j=0;j<(nrowX*NS);j++)
	{
		if(XCOORDS[j]>-99.0){
			g[kk]=GG[j+ncolX*NS];
			kk=kk+1;
		}
	}
	for (i = 0;i < kk;i++) {
//
//	    fprintf(jp,"INITIAL VALUES GRADIENT %5d %5d %12.6f %12.6f\n",i,kk,x[i],g[i]);
//	    printf("INITIAL VALUES GRADIENT %5d %5d %12.6f %12.6f\n",i,kk,x[i],g[i]);
	}

//
//    SIMPLE METHOD OF STOPPING EXECUTION
//
//	istop=100;
//	if(istop==100)idebug=99998;
//	if(idebug==99998)exit(EXIT_FAILURE);
//

	free(ZCOORDSD);
	free(XCOORDSD);
	free(GG);
	return fx;
}

static int progress(
		    void *instance,
		    const lbfgsfloatval_t *x,
		    const lbfgsfloatval_t *g,
		    const lbfgsfloatval_t fx,
		    const lbfgsfloatval_t xnorm,
		    const lbfgsfloatval_t gnorm,
		    const lbfgsfloatval_t step,
		    int n,
		    int k,
		    int ls
		   )
{
	printf("Iteration %d:\n", k);
	printf("  fx = %f, x[0] = %f,  x[1] = %f,  x[2] = %f, x[3] = %f\n", fx, x[0], x[1], x[2], x[3]);
	printf("  xnorm = %f, gnorm = %f, step = %f\n", xnorm, gnorm, step);
	printf("\n");
//
	fprintf(jp,"Iteration %d:\n", k);
	fprintf(jp,"  fx = %f, x[0] = %f,  x[1] = %f,  x[2] = %f, x[3] = %f\n", fx, x[0], x[1], x[2], x[3]);
	fprintf(jp,"  xnorm = %f, gnorm = %f, step = %f\n", xnorm, gnorm, step);
	fprintf(jp,"\n");
	return 0;
}

/*
 *kpnp = number of rows of y
 *kpnq = number of columns of y
rmatrix holds the starting coordinates from the double-centering
yrotate returns the results from L-BFGS (Limited-Memory
       Broyden-Fletcher-Goldfarb-Shanno algorthm
y is the symmetric matrix of dissimilarities (distances)
*/

void mainlbfgs(int kpnp, int kpnq, double *yrotate, double *rmatrix)
{
    double *a, *b;
    int i, j, istop, idebug, kk, ret = 0;
    lbfgsfloatval_t fx;
    lbfgsfloatval_t *x = lbfgs_malloc(N);
    lbfgs_parameter_t param;
    a     = calloc( ((kpnp+kpnq)*NS), sizeof(double));
    b     = calloc( ((kpnp+kpnq)*NS), sizeof(double));
// Transfer coordinates
    for(i=0;i<((kpnq+kpnp)*NS);i++)
    {
	    a[i]=rmatrix[i];
    }
    if (x == NULL) {
        printf("ERROR: Failed to allocate a memory block for variables.\n");
//	return 1;
	return;
    }
    kk=0;
    for(j=0;j<(kpnq*NS);j++)
    {
	    if(CONSTRAINTS[j]<1.0){
//		    XCOORDS[j]=0.0;
	    }
	    if(CONSTRAINTS[j]>0.0){
		    x[kk]=a[j];
		    kk=kk+1;
	    }
    }
    for(j=0;j<(kpnp*NS);j++)
    {
	    if(a[j+(kpnq*NS)]> -99.0){
		    x[kk]=a[j+(kpnq*NS)];
		    kk=kk+1;
	    }
    }
    /* Initialize the variables. */
    for (i = 0;i < kk;i++) {
//
	    fprintf(jp,"INITIAL VALUES %5d %5d %12.6f\n",i,kk,x[i]);
	    printf("INITIAL VALUES %5d %5d %12.6f\n",i,kk,x[i]);
    }

    /* Initialize the parameters for the L-BFGS optimization. */
    lbfgs_parameter_init(&param);
    /*param.linesearch = LBFGS_LINESEARCH_BACKTRACKING;*/

    /*
        Start the L-BFGS optimization; this will invoke the callback functions
        evaluate() and progress() when necessary.
     */
    
//    ret = lbfgs(N, x, &fx, evaluate, progress, NULL, &param);
    ret = lbfgs(kk, x, &fx, evaluate, progress, NULL, &param);

    /* Report the result. */
    printf("L-BFGS optimization terminated with status code = %d\n", ret);
    fprintf(jp,"L-BFGS optimization terminated with status code = %d\n", ret);
//    printf("  fx = %f, x[0] = %f, x[1] = %f\n", fx, x[0], x[1]);
    printf("  fx = %f, x[0] = %f,  x[1] = %f,  x[2] = %f, x[3] = %f\n", fx, x[0], x[1], x[2], x[3]);
    fprintf(jp,"  fx = %f, x[0] = %f,  x[1] = %f,  x[2] = %f, x[3] = %f\n", fx, x[0], x[1], x[2], x[3]);

// PASS BACK THE SOLUTION
//    
    kk=0;
    for(j=0;j<(kpnq*NS);j++)
    {
	    if(CONSTRAINTS[j]<1.0){
		    yrotate[j]=0.0;
	    }
	    if(CONSTRAINTS[j]>0.0){
		    yrotate[j]=x[kk];
		    kk=kk+1;
	    }
    }
    for(j=0;j<(kpnp*NS);j++)
    {
	    yrotate[j+(kpnq*NS)]=a[j+(kpnq*NS)];
	    if(a[j+(kpnq*NS)]> -99.0){
	       yrotate[j+(kpnq*NS)]=x[kk];
	       kk=kk+1;
	    }
    }
//
    lbfgs_free(x);
    free(a);
    free(b);
    return;
}
/*
 *      Limited memory BFGS (L-BFGS).
 *
 * Copyright (c) 1990, Jorge Nocedal
 * Copyright (c) 2007-2010 Naoaki Okazaki
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

/* $Id$ */

/*
This library is a C port of the FORTRAN implementation of Limited-memory
Broyden-Fletcher-Goldfarb-Shanno (L-BFGS) method written by Jorge Nocedal.
The original FORTRAN source code is available at:
http://www.ece.northwestern.edu/~nocedal/lbfgs.html

The L-BFGS algorithm is described in:
    - Jorge Nocedal.
      Updating Quasi-Newton Matrices with Limited Storage.
      <i>Mathematics of Computation</i>, Vol. 35, No. 151, pp. 773--782, 1980.
    - Dong C. Liu and Jorge Nocedal.
      On the limited memory BFGS method for large scale optimization.
      <i>Mathematical Programming</i> B, Vol. 45, No. 3, pp. 503-528, 1989.

The line search algorithms used in this implementation are described in:
    - John E. Dennis and Robert B. Schnabel.
      <i>Numerical Methods for Unconstrained Optimization and Nonlinear
      Equations</i>, Englewood Cliffs, 1983.
    - Jorge J. More and David J. Thuente.
      Line search algorithm with guaranteed sufficient decrease.
      <i>ACM Transactions on Mathematical Software (TOMS)</i>, Vol. 20, No. 3,
      pp. 286-307, 1994.

This library also implements Orthant-Wise Limited-memory Quasi-Newton (OWL-QN)
method presented in:
    - Galen Andrew and Jianfeng Gao.
      Scalable training of L1-regularized log-linear models.
      In <i>Proceedings of the 24th International Conference on Machine
      Learning (ICML 2007)</i>, pp. 33-40, 2007.

I would like to thank the original author, Jorge Nocedal, who has been
distributing the effieicnt and explanatory implementation in an open source
licence.
*/

#ifdef  HAVE_CONFIG_H
#include <config.h>
#endif/*HAVE_CONFIG_H*/

#ifdef  _MSC_VER
#define inline  __inline
#endif/*_MSC_VER*/

#if     defined(USE_SSE) && defined(__SSE2__) && LBFGS_FLOAT == 64
/* Use SSE2 optimization for 64bit double precision. */
#include "arithmetic_sse_double.h"

#elif   defined(USE_SSE) && defined(__SSE__) && LBFGS_FLOAT == 32
/* Use SSE optimization for 32bit float precision. */
#include "arithmetic_sse_float.h"

#else
/* No CPU specific optimization. */
#include "/Users/poole/arithmetic_ansi.h"

#endif

#define min2(a, b)      ((a) <= (b) ? (a) : (b))
#define max2(a, b)      ((a) >= (b) ? (a) : (b))
#define max3(a, b, c)   max2(max2((a), (b)), (c));

struct tag_callback_data {
    int n;
    void *instance;
    lbfgs_evaluate_t proc_evaluate;
    lbfgs_progress_t proc_progress;
};
typedef struct tag_callback_data callback_data_t;

struct tag_iteration_data {
    lbfgsfloatval_t alpha;
    lbfgsfloatval_t *s;     /* [n] */
    lbfgsfloatval_t *y;     /* [n] */
    lbfgsfloatval_t ys;     /* vecdot(y, s) */
};
typedef struct tag_iteration_data iteration_data_t;
// STOPPING CRITERIA -- EPSILON IS THE SECOND NUMBER
static const lbfgs_parameter_t _defparam = {
    6, 1e-4, 0, 1e-5,
    0, LBFGS_LINESEARCH_DEFAULT, 40,
    1e-20, 1e20, 1e-4, 0.9, 0.9, 1.0e-16,
    0.0, 0, -1,
};

/* Forward function declarations. */

typedef int (*line_search_proc)(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wa,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    );
    
static int line_search_backtracking(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wa,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    );

static int line_search_backtracking_owlqn(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wp,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    );

static int line_search_morethuente(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wa,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    );

static int update_trial_interval(
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *fx,
    lbfgsfloatval_t *dx,
    lbfgsfloatval_t *y,
    lbfgsfloatval_t *fy,
    lbfgsfloatval_t *dy,
    lbfgsfloatval_t *t,
    lbfgsfloatval_t *ft,
    lbfgsfloatval_t *dt,
    const lbfgsfloatval_t tmin,
    const lbfgsfloatval_t tmax,
    int *brackt
    );

static lbfgsfloatval_t owlqn_x1norm(
    const lbfgsfloatval_t* x,
    const int start,
    const int n
    );

static void owlqn_pseudo_gradient(
    lbfgsfloatval_t* pg,
    const lbfgsfloatval_t* x,
    const lbfgsfloatval_t* g,
    const int n,
    const lbfgsfloatval_t c,
    const int start,
    const int end
    );

static void owlqn_project(
    lbfgsfloatval_t* d,
    const lbfgsfloatval_t* sign,
    const int start,
    const int end
    );


#if     defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
static int round_out_variables(int n)
{
    n += 7;
    n /= 8;
    n *= 8;
    return n;
}
#endif/*defined(USE_SSE)*/

lbfgsfloatval_t* lbfgs_malloc(int n)
{
#if     defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
    n = round_out_variables(n);
#endif/*defined(USE_SSE)*/
    return (lbfgsfloatval_t*)vecalloc(sizeof(lbfgsfloatval_t) * n);
}

void lbfgs_free(lbfgsfloatval_t *x)
{
    vecfree(x);
}

void lbfgs_parameter_init(lbfgs_parameter_t *param)
{
    memcpy(param, &_defparam, sizeof(*param));
}

int lbfgs(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *ptr_fx,
    lbfgs_evaluate_t proc_evaluate,
    lbfgs_progress_t proc_progress,
    void *instance,
    lbfgs_parameter_t *_param
    )
{
    int ret;
    int i, j, k, ls, end, bound;
    lbfgsfloatval_t step;

    /* Constant parameters and their default values. */
    lbfgs_parameter_t param = (_param != NULL) ? (*_param) : _defparam;
    const int m = param.m;

    lbfgsfloatval_t *xp = NULL;
    lbfgsfloatval_t *g = NULL, *gp = NULL, *pg = NULL;
    lbfgsfloatval_t *d = NULL, *w = NULL, *pf = NULL;
    iteration_data_t *lm = NULL, *it = NULL;
    lbfgsfloatval_t ys, yy;
    lbfgsfloatval_t xnorm, gnorm, beta;
    lbfgsfloatval_t fx = 0.;
    lbfgsfloatval_t rate = 0.;
    line_search_proc linesearch = line_search_morethuente;

    /* Construct a callback data. */
    callback_data_t cd;
    cd.n = n;
    cd.instance = instance;
    cd.proc_evaluate = proc_evaluate;
    cd.proc_progress = proc_progress;

#if     defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
    /* Round out the number of variables. */
    n = round_out_variables(n);
#endif/*defined(USE_SSE)*/

    /* Check the input parameters for errors. */
    if (n <= 0) {
        return LBFGSERR_INVALID_N;
    }
#if     defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
    if (n % 8 != 0) {
        return LBFGSERR_INVALID_N_SSE;
    }
    if ((uintptr_t)(const void*)x % 16 != 0) {
        return LBFGSERR_INVALID_X_SSE;
    }
#endif/*defined(USE_SSE)*/
    fprintf(kp,"PARAMETERS %8d\n",param.m);
    fprintf(kp,"PARAMETERS %8d %15.8f\n",param.m, param.epsilon);
//    fprintf(kp,"PARAMETERS %8d %15.8f %15.8f %15.8f %8d\n",param.m, param.epsilon, param.past);
//    fprintf(kp,"PARAMETERS %8d %15.8f %15.8f %8d\n",param.m, param.epsilon, param.delta, param.min_step);
    fprintf(kp,"PARAMETERS %8d %15.8f %15.8f %15.8f %15.8f %15.8f\n",param.m, param.epsilon, param.max_step, param.ftol, param.gtol, param.xtol);
    if (param.epsilon < 0.) {
        return LBFGSERR_INVALID_EPSILON;
    }
    if (param.past < 0) {
        return LBFGSERR_INVALID_TESTPERIOD;
    }
    if (param.delta < 0.) {
        return LBFGSERR_INVALID_DELTA;
    }
    if (param.min_step < 0.) {
        return LBFGSERR_INVALID_MINSTEP;
    }
    if (param.max_step < param.min_step) {
        return LBFGSERR_INVALID_MAXSTEP;
    }
    if (param.ftol < 0.) {
        return LBFGSERR_INVALID_FTOL;
    }
    if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE ||
        param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE) {
        if (param.wolfe <= param.ftol || 1. <= param.wolfe) {
            return LBFGSERR_INVALID_WOLFE;
        }
    }
    if (param.gtol < 0.) {
        return LBFGSERR_INVALID_GTOL;
    }
    if (param.xtol < 0.) {
        return LBFGSERR_INVALID_XTOL;
    }
    if (param.max_linesearch <= 0) {
        return LBFGSERR_INVALID_MAXLINESEARCH;
    }
    if (param.orthantwise_c < 0.) {
        return LBFGSERR_INVALID_ORTHANTWISE;
    }
    if (param.orthantwise_start < 0 || n < param.orthantwise_start) {
        return LBFGSERR_INVALID_ORTHANTWISE_START;
    }
    if (param.orthantwise_end < 0) {
        param.orthantwise_end = n;
    }
    if (n < param.orthantwise_end) {
        return LBFGSERR_INVALID_ORTHANTWISE_END;
    }
    if (param.orthantwise_c != 0.) {
        switch (param.linesearch) {
        case LBFGS_LINESEARCH_BACKTRACKING:
            linesearch = line_search_backtracking_owlqn;
            break;
        default:
            /* Only the backtracking method is available. */
            return LBFGSERR_INVALID_LINESEARCH;
        }
    } else {
        switch (param.linesearch) {
        case LBFGS_LINESEARCH_MORETHUENTE:
            linesearch = line_search_morethuente;
            break;
        case LBFGS_LINESEARCH_BACKTRACKING_ARMIJO:
        case LBFGS_LINESEARCH_BACKTRACKING_WOLFE:
        case LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE:
            linesearch = line_search_backtracking;
            break;
        default:
            return LBFGSERR_INVALID_LINESEARCH;
        }
    }

    /* Allocate working space. */
    xp = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
    g = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
    gp = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
    d = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
    w = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
    if (xp == NULL || g == NULL || gp == NULL || d == NULL || w == NULL) {
        ret = LBFGSERR_OUTOFMEMORY;
        goto lbfgs_exit;
    }

    if (param.orthantwise_c != 0.) {
        /* Allocate working space for OW-LQN. */
        pg = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
        if (pg == NULL) {
            ret = LBFGSERR_OUTOFMEMORY;
            goto lbfgs_exit;
        }
    }

    /* Allocate limited memory storage. */
    lm = (iteration_data_t*)vecalloc(m * sizeof(iteration_data_t));
    if (lm == NULL) {
        ret = LBFGSERR_OUTOFMEMORY;
        goto lbfgs_exit;
    }

    /* Initialize the limited memory. */
    for (i = 0;i < m;++i) {
        it = &lm[i];
        it->alpha = 0;
        it->ys = 0;
        it->s = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
        it->y = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
        if (it->s == NULL || it->y == NULL) {
            ret = LBFGSERR_OUTOFMEMORY;
            goto lbfgs_exit;
        }
    }

    /* Allocate an array for storing previous values of the objective function. */
    if (0 < param.past) {
        pf = (lbfgsfloatval_t*)vecalloc(param.past * sizeof(lbfgsfloatval_t));
    }

    /* Evaluate the function value and its gradient. */
    fx = cd.proc_evaluate(cd.instance, x, g, cd.n, 0);
    if (0. != param.orthantwise_c) {
        /* Compute the L1 norm of the variable and add it to the object value. */
        xnorm = owlqn_x1norm(x, param.orthantwise_start, param.orthantwise_end);
        fx += xnorm * param.orthantwise_c;
        owlqn_pseudo_gradient(
            pg, x, g, n,
            param.orthantwise_c, param.orthantwise_start, param.orthantwise_end
            );
    }

    /* Store the initial value of the objective function. */
    if (pf != NULL) {
        pf[0] = fx;
    }

    /*
        Compute the direction;
        we assume the initial hessian matrix H_0 as the identity matrix.
     */
    if (param.orthantwise_c == 0.) {
        vecncpy(d, g, n);
    } else {
        vecncpy(d, pg, n);
    }

    /*
       Make sure that the initial variables are not a minimizer.
     */
    vec2norm(&xnorm, x, n);
    if (param.orthantwise_c == 0.) {
        vec2norm(&gnorm, g, n);
    } else {
        vec2norm(&gnorm, pg, n);
    }
    if (xnorm < 1.0) xnorm = 1.0;
    if (gnorm / xnorm <= param.epsilon) {
        ret = LBFGS_ALREADY_MINIMIZED;
        goto lbfgs_exit;
    }

    /* Compute the initial step:
        step = 1.0 / sqrt(vecdot(d, d, n))
     */
    vec2norminv(&step, d, n);

    k = 1;
    end = 0;
    for (;;) {
        /* Store the current position and gradient vectors. */
        veccpy(xp, x, n);
        veccpy(gp, g, n);

        /* Search for an optimal step. */
        if (param.orthantwise_c == 0.) {
            ls = linesearch(n, x, &fx, g, d, &step, xp, gp, w, &cd, &param);
        } else {
            ls = linesearch(n, x, &fx, g, d, &step, xp, pg, w, &cd, &param);
            owlqn_pseudo_gradient(
                pg, x, g, n,
                param.orthantwise_c, param.orthantwise_start, param.orthantwise_end
                );
        }
        if (ls < 0) {
            /* Revert to the previous point. */
            veccpy(x, xp, n);
            veccpy(g, gp, n);
            ret = ls;
            goto lbfgs_exit;
        }

        /* Compute x and g norms. */
        vec2norm(&xnorm, x, n);
        if (param.orthantwise_c == 0.) {
            vec2norm(&gnorm, g, n);
        } else {
            vec2norm(&gnorm, pg, n);
        }

        /* Report the progress. */
        if (cd.proc_progress) {
            if ((ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls))) {
                goto lbfgs_exit;
            }
        }

        /*
            Convergence test.
            The criterion is given by the following formula:
                |g(x)| / \max(1, |x|) < \epsilon
         */
        if (xnorm < 1.0) xnorm = 1.0;
        if (gnorm / xnorm <= param.epsilon) {
            /* Convergence. */
            ret = LBFGS_SUCCESS;
            break;
        }

        /*
            Test for stopping criterion.
            The criterion is given by the following formula:
                (f(past_x) - f(x)) / f(x) < \delta
         */
        if (pf != NULL) {
            /* We don't test the stopping criterion while k < past. */
            if (param.past <= k) {
                /* Compute the relative improvement from the past. */
                rate = (pf[k % param.past] - fx) / fx;

                /* The stopping criterion. */
                if (rate < param.delta) {
                    ret = LBFGS_STOP;
                    break;
                }
            }

            /* Store the current value of the objective function. */
            pf[k % param.past] = fx;
        }

        if (param.max_iterations != 0 && param.max_iterations < k+1) {
            /* Maximum number of iterations. */
            ret = LBFGSERR_MAXIMUMITERATION;
            break;
        }

        /*
            Update vectors s and y:
                s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
                y_{k+1} = g_{k+1} - g_{k}.
         */
        it = &lm[end];
        vecdiff(it->s, x, xp, n);
        vecdiff(it->y, g, gp, n);

        /*
            Compute scalars ys and yy:
                ys = y^t \cdot s = 1 / \rho.
                yy = y^t \cdot y.
            Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
         */
        vecdot(&ys, it->y, it->s, n);
        vecdot(&yy, it->y, it->y, n);
        it->ys = ys;

        /*
            Recursive formula to compute dir = -(H \cdot g).
                This is described in page 779 of:
                Jorge Nocedal.
                Updating Quasi-Newton Matrices with Limited Storage.
                Mathematics of Computation, Vol. 35, No. 151,
                pp. 773--782, 1980.
         */
        bound = (m <= k) ? m : k;
        ++k;
        end = (end + 1) % m;

        /* Compute the steepest direction. */
        if (param.orthantwise_c == 0.) {
            /* Compute the negative of gradients. */
            vecncpy(d, g, n);
        } else {
            vecncpy(d, pg, n);
        }

        j = end;
        for (i = 0;i < bound;++i) {
            j = (j + m - 1) % m;    /* if (--j == -1) j = m-1; */
            it = &lm[j];
            /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
            vecdot(&it->alpha, it->s, d, n);
            it->alpha /= it->ys;
            /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
            vecadd(d, it->y, -it->alpha, n);
        }

        vecscale(d, ys / yy, n);

        for (i = 0;i < bound;++i) {
            it = &lm[j];
            /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamma_{i}. */
            vecdot(&beta, it->y, d, n);
            beta /= it->ys;
            /* \gamma_{i+1} = \gamma_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
            vecadd(d, it->s, it->alpha - beta, n);
            j = (j + 1) % m;        /* if (++j == m) j = 0; */
        }

        /*
            Constrain the search direction for orthant-wise updates.
         */
        if (param.orthantwise_c != 0.) {
            for (i = param.orthantwise_start;i < param.orthantwise_end;++i) {
                if (d[i] * pg[i] >= 0) {
                    d[i] = 0;
                }
            }
        }

        /*
            Now the search direction d is ready. We try step = 1 first.
         */
        step = 1.0;
    }

lbfgs_exit:
    /* Return the final value of the objective function. */
    if (ptr_fx != NULL) {
        *ptr_fx = fx;
    }

    vecfree(pf);

    /* Free memory blocks used by this function. */
    if (lm != NULL) {
        for (i = 0;i < m;++i) {
            vecfree(lm[i].s);
            vecfree(lm[i].y);
        }
        vecfree(lm);
    }
    vecfree(pg);
    vecfree(w);
    vecfree(d);
    vecfree(gp);
    vecfree(g);
    vecfree(xp);

    return ret;
}



static int line_search_backtracking(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wp,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    )
{
    int count = 0;
    lbfgsfloatval_t width, dg;
    lbfgsfloatval_t finit, dginit = 0., dgtest;
    const lbfgsfloatval_t dec = 0.5, inc = 2.1;

    /* Check the input parameters for errors. */
    if (*stp <= 0.) {
        return LBFGSERR_INVALIDPARAMETERS;
    }

    /* Compute the initial gradient in the search direction. */
    vecdot(&dginit, g, s, n);

    /* Make sure that s points to a descent direction. */
    if (0 < dginit) {
        return LBFGSERR_INCREASEGRADIENT;
    }

    /* The initial value of the objective function. */
    finit = *f;
    dgtest = param->ftol * dginit;

    for (;;) {
        veccpy(x, xp, n);
        vecadd(x, s, *stp, n);

        /* Evaluate the function and gradient values. */
        *f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);

        ++count;

        if (*f > finit + *stp * dgtest) {
            width = dec;
        } else {
            /* The sufficient decrease condition (Armijo condition). */
            if (param->linesearch == LBFGS_LINESEARCH_BACKTRACKING_ARMIJO) {
                /* Exit with the Armijo condition. */
                return count;
	        }

	        /* Check the Wolfe condition. */
	        vecdot(&dg, g, s, n);
	        if (dg < param->wolfe * dginit) {
    		    width = inc;
	        } else {
		        if(param->linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE) {
		            /* Exit with the regular Wolfe condition. */
		            return count;
		        }

		        /* Check the strong Wolfe condition. */
		        if(dg > -param->wolfe * dginit) {
		            width = dec;
		        } else {
		            /* Exit with the strong Wolfe condition. */
		            return count;
		        }
            }
        }

        if (*stp < param->min_step) {
            /* The step is the minimum value. */
            return LBFGSERR_MINIMUMSTEP;
        }
        if (*stp > param->max_step) {
            /* The step is the maximum value. */
            return LBFGSERR_MAXIMUMSTEP;
        }
        if (param->max_linesearch <= count) {
            /* Maximum number of iteration. */
            return LBFGSERR_MAXIMUMLINESEARCH;
        }

        (*stp) *= width;
    }
}



static int line_search_backtracking_owlqn(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wp,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    )
{
    int i, count = 0;
    lbfgsfloatval_t width = 0.5, norm = 0.;
    lbfgsfloatval_t finit = *f, dgtest;

    /* Check the input parameters for errors. */
    if (*stp <= 0.) {
        return LBFGSERR_INVALIDPARAMETERS;
    }

    /* Choose the orthant for the new point. */
    for (i = 0;i < n;++i) {
        wp[i] = (xp[i] == 0.) ? -gp[i] : xp[i];
    }

    for (;;) {
        /* Update the current point. */
        veccpy(x, xp, n);
        vecadd(x, s, *stp, n);

        /* The current point is projected onto the orthant. */
        owlqn_project(x, wp, param->orthantwise_start, param->orthantwise_end);

        /* Evaluate the function and gradient values. */
        *f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);

        /* Compute the L1 norm of the variables and add it to the object value. */
        norm = owlqn_x1norm(x, param->orthantwise_start, param->orthantwise_end);
        *f += norm * param->orthantwise_c;

        ++count;

        dgtest = 0.;
        for (i = 0;i < n;++i) {
            dgtest += (x[i] - xp[i]) * gp[i];
        }

        if (*f <= finit + param->ftol * dgtest) {
            /* The sufficient decrease condition. */
            return count;
        }

        if (*stp < param->min_step) {
            /* The step is the minimum value. */
            return LBFGSERR_MINIMUMSTEP;
        }
        if (*stp > param->max_step) {
            /* The step is the maximum value. */
            return LBFGSERR_MAXIMUMSTEP;
        }
        if (param->max_linesearch <= count) {
            /* Maximum number of iteration. */
            return LBFGSERR_MAXIMUMLINESEARCH;
        }

        (*stp) *= width;
    }
}



static int line_search_morethuente(
    int n,
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *f,
    lbfgsfloatval_t *g,
    lbfgsfloatval_t *s,
    lbfgsfloatval_t *stp,
    const lbfgsfloatval_t* xp,
    const lbfgsfloatval_t* gp,
    lbfgsfloatval_t *wa,
    callback_data_t *cd,
    const lbfgs_parameter_t *param
    )
{
    int count = 0;
    int brackt, stage1, uinfo = 0;
    lbfgsfloatval_t dg;
    lbfgsfloatval_t stx, fx, dgx;
    lbfgsfloatval_t sty, fy, dgy;
    lbfgsfloatval_t fxm, dgxm, fym, dgym, fm, dgm;
    lbfgsfloatval_t finit, ftest1, dginit, dgtest;
    lbfgsfloatval_t width, prev_width;
    lbfgsfloatval_t stmin, stmax;

    /* Check the input parameters for errors. */
    if (*stp <= 0.) {
        return LBFGSERR_INVALIDPARAMETERS;
    }

    /* Compute the initial gradient in the search direction. */
    vecdot(&dginit, g, s, n);

    /* Make sure that s points to a descent direction. */
    if (0 < dginit) {
        return LBFGSERR_INCREASEGRADIENT;
    }

    /* Initialize local variables. */
    brackt = 0;
    stage1 = 1;
    finit = *f;
    dgtest = param->ftol * dginit;
    width = param->max_step - param->min_step;
    prev_width = 2.0 * width;

    /*
        The variables stx, fx, dgx contain the values of the step,
        function, and directional derivative at the best step.
        The variables sty, fy, dgy contain the value of the step,
        function, and derivative at the other endpoint of
        the interval of uncertainty.
        The variables stp, f, dg contain the values of the step,
        function, and derivative at the current step.
    */
    stx = sty = 0.;
    fx = fy = finit;
    dgx = dgy = dginit;

    for (;;) {
        /*
            Set the minimum and maximum steps to correspond to the
            present interval of uncertainty.
         */
        if (brackt) {
            stmin = min2(stx, sty);
            stmax = max2(stx, sty);
        } else {
            stmin = stx;
            stmax = *stp + 4.0 * (*stp - stx);
        }

        /* Clip the step in the range of [stpmin, stpmax]. */
        if (*stp < param->min_step) *stp = param->min_step;
        if (param->max_step < *stp) *stp = param->max_step;

        /*
            If an unusual termination is to occur then let
            stp be the lowest point obtained so far.
         */
        if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) {
            *stp = stx;
        }

        /*
            Compute the current value of x:
                x <- x + (*stp) * s.
         */
        veccpy(x, xp, n);
        vecadd(x, s, *stp, n);

        /* Evaluate the function and gradient values. */
        *f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);
        vecdot(&dg, g, s, n);

        ftest1 = finit + *stp * dgtest;
        ++count;

        /* Test for errors and convergence. */
        if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) {
            /* Rounding errors prevent further progress. */
            return LBFGSERR_ROUNDING_ERROR;
        }
        if (*stp == param->max_step && *f <= ftest1 && dg <= dgtest) {
            /* The step is the maximum value. */
            return LBFGSERR_MAXIMUMSTEP;
        }
        if (*stp == param->min_step && (ftest1 < *f || dgtest <= dg)) {
            /* The step is the minimum value. */
            return LBFGSERR_MINIMUMSTEP;
        }
        if (brackt && (stmax - stmin) <= param->xtol * stmax) {
            /* Relative width of the interval of uncertainty is at most xtol. */
            return LBFGSERR_WIDTHTOOSMALL;
        }
        if (param->max_linesearch <= count) {
            /* Maximum number of iteration. */
            return LBFGSERR_MAXIMUMLINESEARCH;
        }
        if (*f <= ftest1 && fabs(dg) <= param->gtol * (-dginit)) {
            /* The sufficient decrease condition and the directional derivative condition hold. */
            return count;
        }

        /*
            In the first stage we seek a step for which the modified
            function has a nonpositive value and nonnegative derivative.
         */
        if (stage1 && *f <= ftest1 && min2(param->ftol, param->gtol) * dginit <= dg) {
            stage1 = 0;
        }

        /*
            A modified function is used to predict the step only if
            we have not obtained a step for which the modified
            function has a nonpositive function value and nonnegative
            derivative, and if a lower function value has been
            obtained but the decrease is not sufficient.
         */
        if (stage1 && ftest1 < *f && *f <= fx) {
            /* Define the modified function and derivative values. */
            fm = *f - *stp * dgtest;
            fxm = fx - stx * dgtest;
            fym = fy - sty * dgtest;
            dgm = dg - dgtest;
            dgxm = dgx - dgtest;
            dgym = dgy - dgtest;

            /*
                Call update_trial_interval() to update the interval of
                uncertainty and to compute the new step.
             */
            uinfo = update_trial_interval(
                &stx, &fxm, &dgxm,
                &sty, &fym, &dgym,
                stp, &fm, &dgm,
                stmin, stmax, &brackt
                );

            /* Reset the function and gradient values for f. */
            fx = fxm + stx * dgtest;
            fy = fym + sty * dgtest;
            dgx = dgxm + dgtest;
            dgy = dgym + dgtest;
        } else {
            /*
                Call update_trial_interval() to update the interval of
                uncertainty and to compute the new step.
             */
            uinfo = update_trial_interval(
                &stx, &fx, &dgx,
                &sty, &fy, &dgy,
                stp, f, &dg,
                stmin, stmax, &brackt
                );
        }

        /*
            Force a sufficient decrease in the interval of uncertainty.
         */
        if (brackt) {
            if (0.66 * prev_width <= fabs(sty - stx)) {
                *stp = stx + 0.5 * (sty - stx);
            }
            prev_width = width;
            width = fabs(sty - stx);
        }
    }

    return LBFGSERR_LOGICERROR;
}



/**
 * Define the local variables for computing minimizers.
 */
#define USES_MINIMIZER \
    lbfgsfloatval_t a, d, gamma, theta, p, q, r, s;

/**
 * Find a minimizer of an interpolated cubic function.
 *  @param  cm      The minimizer of the interpolated cubic.
 *  @param  u       The value of one point, u.
 *  @param  fu      The value of f(u).
 *  @param  du      The value of f'(u).
 *  @param  v       The value of another point, v.
 *  @param  fv      The value of f(v).
 *  @param  du      The value of f'(v).
 */
#define CUBIC_MINIMIZER(cm, u, fu, du, v, fv, dv) \
    d = (v) - (u); \
    theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \
    p = fabs(theta); \
    q = fabs(du); \
    r = fabs(dv); \
    s = max3(p, q, r); \
    /* gamma = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \
    a = theta / s; \
    gamma = s * sqrt(a * a - ((du) / s) * ((dv) / s)); \
    if ((v) < (u)) gamma = -gamma; \
    p = gamma - (du) + theta; \
    q = gamma - (du) + gamma + (dv); \
    r = p / q; \
    (cm) = (u) + r * d;

/**
 * Find a minimizer of an interpolated cubic function.
 *  @param  cm      The minimizer of the interpolated cubic.
 *  @param  u       The value of one point, u.
 *  @param  fu      The value of f(u).
 *  @param  du      The value of f'(u).
 *  @param  v       The value of another point, v.
 *  @param  fv      The value of f(v).
 *  @param  du      The value of f'(v).
 *  @param  xmin    The maximum value.
 *  @param  xmin    The minimum value.
 */
#define CUBIC_MINIMIZER2(cm, u, fu, du, v, fv, dv, xmin, xmax) \
    d = (v) - (u); \
    theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \
    p = fabs(theta); \
    q = fabs(du); \
    r = fabs(dv); \
    s = max3(p, q, r); \
    /* gamma = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \
    a = theta / s; \
    gamma = s * sqrt(max2(0, a * a - ((du) / s) * ((dv) / s))); \
    if ((u) < (v)) gamma = -gamma; \
    p = gamma - (dv) + theta; \
    q = gamma - (dv) + gamma + (du); \
    r = p / q; \
    if (r < 0. && gamma != 0.) { \
        (cm) = (v) - r * d; \
    } else if (a < 0) { \
        (cm) = (xmax); \
    } else { \
        (cm) = (xmin); \
    }

/**
 * Find a minimizer of an interpolated quadratic function.
 *  @param  qm      The minimizer of the interpolated quadratic.
 *  @param  u       The value of one point, u.
 *  @param  fu      The value of f(u).
 *  @param  du      The value of f'(u).
 *  @param  v       The value of another point, v.
 *  @param  fv      The value of f(v).
 */
#define QUARD_MINIMIZER(qm, u, fu, du, v, fv) \
    a = (v) - (u); \
    (qm) = (u) + (du) / (((fu) - (fv)) / a + (du)) / 2 * a;

/**
 * Find a minimizer of an interpolated quadratic function.
 *  @param  qm      The minimizer of the interpolated quadratic.
 *  @param  u       The value of one point, u.
 *  @param  du      The value of f'(u).
 *  @param  v       The value of another point, v.
 *  @param  dv      The value of f'(v).
 */
#define QUARD_MINIMIZER2(qm, u, du, v, dv) \
    a = (u) - (v); \
    (qm) = (v) + (dv) / ((dv) - (du)) * a;

/**
 * Update a safeguarded trial value and interval for line search.
 *
 *  The parameter x represents the step with the least function value.
 *  The parameter t represents the current step. This function assumes
 *  that the derivative at the point of x in the direction of the step.
 *  If the bracket is set to true, the minimizer has been bracketed in
 *  an interval of uncertainty with endpoints between x and y.
 *
 *  @param  x       The pointer to the value of one endpoint.
 *  @param  fx      The pointer to the value of f(x).
 *  @param  dx      The pointer to the value of f'(x).
 *  @param  y       The pointer to the value of another endpoint.
 *  @param  fy      The pointer to the value of f(y).
 *  @param  dy      The pointer to the value of f'(y).
 *  @param  t       The pointer to the value of the trial value, t.
 *  @param  ft      The pointer to the value of f(t).
 *  @param  dt      The pointer to the value of f'(t).
 *  @param  tmin    The minimum value for the trial value, t.
 *  @param  tmax    The maximum value for the trial value, t.
 *  @param  brackt  The pointer to the predicate if the trial value is
 *                  bracketed.
 *  @retval int     Status value. Zero indicates a normal termination.
 *  
 *  @see
 *      Jorge J. More and David J. Thuente. Line search algorithm with
 *      guaranteed sufficient decrease. ACM Transactions on Mathematical
 *      Software (TOMS), Vol 20, No 3, pp. 286-307, 1994.
 */
static int update_trial_interval(
    lbfgsfloatval_t *x,
    lbfgsfloatval_t *fx,
    lbfgsfloatval_t *dx,
    lbfgsfloatval_t *y,
    lbfgsfloatval_t *fy,
    lbfgsfloatval_t *dy,
    lbfgsfloatval_t *t,
    lbfgsfloatval_t *ft,
    lbfgsfloatval_t *dt,
    const lbfgsfloatval_t tmin,
    const lbfgsfloatval_t tmax,
    int *brackt
    )
{
    int bound;
    int dsign = fsigndiff(dt, dx);
    lbfgsfloatval_t mc; /* minimizer of an interpolated cubic. */
    lbfgsfloatval_t mq; /* minimizer of an interpolated quadratic. */
    lbfgsfloatval_t newt;   /* new trial value. */
    USES_MINIMIZER;     /* for CUBIC_MINIMIZER and QUARD_MINIMIZER. */

    /* Check the input parameters for errors. */
    if (*brackt) {
        if (*t <= min2(*x, *y) || max2(*x, *y) <= *t) {
            /* The trival value t is out of the interval. */
            return LBFGSERR_OUTOFINTERVAL;
        }
        if (0. <= *dx * (*t - *x)) {
            /* The function must decrease from x. */
            return LBFGSERR_INCREASEGRADIENT;
        }
        if (tmax < tmin) {
            /* Incorrect tmin and tmax specified. */
            return LBFGSERR_INCORRECT_TMINMAX;
        }
    }

    /*
        Trial value selection.
     */
    if (*fx < *ft) {
        /*
            Case 1: a higher function value.
            The minimum is brackt. If the cubic minimizer is closer
            to x than the quadratic one, the cubic one is taken, else
            the average of the minimizers is taken.
         */
        *brackt = 1;
        bound = 1;
        CUBIC_MINIMIZER(mc, *x, *fx, *dx, *t, *ft, *dt);
        QUARD_MINIMIZER(mq, *x, *fx, *dx, *t, *ft);
        if (fabs(mc - *x) < fabs(mq - *x)) {
            newt = mc;
        } else {
            newt = mc + 0.5 * (mq - mc);
        }
    } else if (dsign) {
        /*
            Case 2: a lower function value and derivatives of
            opposite sign. The minimum is brackt. If the cubic
            minimizer is closer to x than the quadratic (secant) one,
            the cubic one is taken, else the quadratic one is taken.
         */
        *brackt = 1;
        bound = 0;
        CUBIC_MINIMIZER(mc, *x, *fx, *dx, *t, *ft, *dt);
        QUARD_MINIMIZER2(mq, *x, *dx, *t, *dt);
        if (fabs(mc - *t) > fabs(mq - *t)) {
            newt = mc;
        } else {
            newt = mq;
        }
    } else if (fabs(*dt) < fabs(*dx)) {
        /*
            Case 3: a lower function value, derivatives of the
            same sign, and the magnitude of the derivative decreases.
            The cubic minimizer is only used if the cubic tends to
            infinity in the direction of the minimizer or if the minimum
            of the cubic is beyond t. Otherwise the cubic minimizer is
            defined to be either tmin or tmax. The quadratic (secant)
            minimizer is also computed and if the minimum is brackt
            then the the minimizer closest to x is taken, else the one
            farthest away is taken.
         */
        bound = 1;
        CUBIC_MINIMIZER2(mc, *x, *fx, *dx, *t, *ft, *dt, tmin, tmax);
        QUARD_MINIMIZER2(mq, *x, *dx, *t, *dt);
        if (*brackt) {
            if (fabs(*t - mc) < fabs(*t - mq)) {
                newt = mc;
            } else {
                newt = mq;
            }
        } else {
            if (fabs(*t - mc) > fabs(*t - mq)) {
                newt = mc;
            } else {
                newt = mq;
            }
        }
    } else {
        /*
            Case 4: a lower function value, derivatives of the
            same sign, and the magnitude of the derivative does
            not decrease. If the minimum is not brackt, the step
            is either tmin or tmax, else the cubic minimizer is taken.
         */
        bound = 0;
        if (*brackt) {
            CUBIC_MINIMIZER(newt, *t, *ft, *dt, *y, *fy, *dy);
        } else if (*x < *t) {
            newt = tmax;
        } else {
            newt = tmin;
        }
    }

    /*
        Update the interval of uncertainty. This update does not
        depend on the new step or the case analysis above.

        - Case a: if f(x) < f(t),
            x <- x, y <- t.
        - Case b: if f(t) <= f(x) && f'(t)*f'(x) > 0,
            x <- t, y <- y.
        - Case c: if f(t) <= f(x) && f'(t)*f'(x) < 0, 
            x <- t, y <- x.
     */
    if (*fx < *ft) {
        /* Case a */
        *y = *t;
        *fy = *ft;
        *dy = *dt;
    } else {
        /* Case c */
        if (dsign) {
            *y = *x;
            *fy = *fx;
            *dy = *dx;
        }
        /* Cases b and c */
        *x = *t;
        *fx = *ft;
        *dx = *dt;
    }

    /* Clip the new trial value in [tmin, tmax]. */
    if (tmax < newt) newt = tmax;
    if (newt < tmin) newt = tmin;

    /*
        Redefine the new trial value if it is close to the upper bound
        of the interval.
     */
    if (*brackt && bound) {
        mq = *x + 0.66 * (*y - *x);
        if (*x < *y) {
            if (mq < newt) newt = mq;
        } else {
            if (newt < mq) newt = mq;
        }
    }

    /* Return the new trial value. */
    *t = newt;
    return 0;
}





static lbfgsfloatval_t owlqn_x1norm(
    const lbfgsfloatval_t* x,
    const int start,
    const int n
    )
{
    int i;
    lbfgsfloatval_t norm = 0.;

    for (i = start;i < n;++i) {
        norm += fabs(x[i]);
    }

    return norm;
}

static void owlqn_pseudo_gradient(
    lbfgsfloatval_t* pg,
    const lbfgsfloatval_t* x,
    const lbfgsfloatval_t* g,
    const int n,
    const lbfgsfloatval_t c,
    const int start,
    const int end
    )
{
    int i;

    /* Compute the negative of gradients. */
    for (i = 0;i < start;++i) {
        pg[i] = g[i];
    }

    /* Compute the psuedo-gradients. */
    for (i = start;i < end;++i) {
        if (x[i] < 0.) {
            /* Differentiable. */
            pg[i] = g[i] - c;
        } else if (0. < x[i]) {
            /* Differentiable. */
            pg[i] = g[i] + c;
        } else {
            if (g[i] < -c) {
                /* Take the right partial derivative. */
                pg[i] = g[i] + c;
            } else if (c < g[i]) {
                /* Take the left partial derivative. */
                pg[i] = g[i] - c;
            } else {
                pg[i] = 0.;
            }
        }
    }

    for (i = end;i < n;++i) {
        pg[i] = g[i];
    }
}

static void owlqn_project(
    lbfgsfloatval_t* d,
    const lbfgsfloatval_t* sign,
    const int start,
    const int end
    )
{
    int i;

    for (i = start;i < end;++i) {
        if (d[i] * sign[i] <= 0) {
            d[i] = 0;
        }
    }
}
